Commit 0b92bb68 authored by Felix Mauch's avatar Felix Mauch
Browse files

added ur3e and ur5e

parent 1f7aabf4
kinematics:
shoulder:
x: 0
y: 0
z: 0.15185
roll: -0
pitch: 0
yaw: -0
upper_arm:
x: 0
y: 0
z: 0
roll: 1.570796327
pitch: 0
yaw: -0
forearm:
x: -0.24355
y: 0
z: 0
roll: -0
pitch: 0
yaw: -0
wrist_1:
x: -0.2132
y: 0
z: 0.13105
roll: -0
pitch: 0
yaw: -0
wrist_2:
x: 0
y: -0.08535
z: -1.750557762378351e-11
roll: 1.570796327
pitch: 0
yaw: -0
wrist_3:
x: 0
y: 0.0921
z: -1.8890025766262e-11
roll: 1.570796326589793
pitch: 3.141592653589793
yaw: 3.141592653589793
hash: calib_16756443741236045476
\ No newline at end of file
kinematics:
shoulder:
x: 0
y: 0
z: 0.1625
roll: -0
pitch: 0
yaw: -0
upper_arm:
x: 0
y: 0
z: 0
roll: 1.570796327
pitch: 0
yaw: -0
forearm:
x: -0.425
y: 0
z: 0
roll: -0
pitch: 0
yaw: -0
wrist_1:
x: -0.3922
y: 0
z: 0.1333
roll: -0
pitch: 0
yaw: -0
wrist_2:
x: 0
y: -0.0997
z: -2.044881182297852e-11
roll: 1.570796327
pitch: 0
yaw: -0
wrist_3:
x: 0
y: 0.09959999999999999
z: -2.042830148012698e-11
roll: 1.570796326589793
pitch: 3.141592653589793
yaw: 3.141592653589793
hash: calib_12788084448423163542
\ No newline at end of file
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur3e_robot.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur3e_joint_limited_robot.urdf.xacro'" />
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur3e_default.yaml"/>
<param unless="$(arg limited)" name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur3e_robot.urdf.xacro'
kinematics_config:=$(arg kinematics_config)"
/>
<param if="$(arg limited)" name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur3e_joint_limited_robot.urdf.xacro'
kinematics_config:=$(arg kinematics_config)"
/>
</launch>
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur5e_robot.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur5e_joint_limited_robot.urdf.xacro'" />
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur5e_default.yaml"/>
<param unless="$(arg limited)" name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur5e_robot.urdf.xacro'
kinematics_config:=$(arg kinematics_config)"
/>
<param if="$(arg limited)" name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur5e_joint_limited_robot.urdf.xacro'
kinematics_config:=$(arg kinematics_config)"
/>
</launch>
......@@ -8,7 +8,7 @@
<xacro:include filename="$(find ur_e_description)/urdf/ur.transmission.xacro" />
<xacro:include filename="$(find ur_e_description)/urdf/ur.gazebo.xacro" />
<xacro:macro name="cylinder_inertial" params="radius length mass *origin">
<xacro:macro name="cylinder_inertial" params="radius length mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
......@@ -19,12 +19,15 @@
</xacro:macro>
<xacro:macro name="ur3e_robot" params="prefix joint_limited
shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}">
shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
kinematics_file">
<xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="2.0" /> <!-- This mass might be incorrect -->
......@@ -46,29 +49,17 @@
<xacro:property name="wrist_2_cog" value="0.0 0.0 0.01" />
<xacro:property name="wrist_3_cog" value="0.0 0.0 -0.02" />
<!-- Kinematic model -->
<!-- Properties from urcontrol.conf -->
<xacro:property name="d1" value="0.152" />
<xacro:property name="a2" value="-0.244" />
<xacro:property name="a3" value="-0.213" />
<xacro:property name="d4" value="0.104" />
<xacro:property name="d5" value="0.085" />
<xacro:property name="d6" value="0.092" />
<!-- Arbitrary offsets for shoulder/elbow joints -->
<xacro:property name="shoulder_offset" value="0.120" /> <!-- measured from model -->
<xacro:property name="elbow_offset" value="-0.093" /> <!-- measured from model -->
<xacro:property name="elbow_offset" value="0.027" /> <!-- measured from model -->
<xacro:property name="upper_arm_inertia_offset" value="0.12" /> <!-- measured from model -->
<!-- link lengths used in model -->
<xacro:property name="shoulder_height" value="${d1}" />
<xacro:property name="upper_arm_length" value="${-a2}" />
<xacro:property name="forearm_length" value="${-a3}" />
<xacro:property name="wrist_1_length" value="${d4}" />
<xacro:property name="wrist_2_length" value="${d5}" />
<xacro:property name="wrist_3_length" value="${d6}" />
<!-- This is for compatibility reasons, as transformation between base and base_link was always 180 degree in the ur_description -->
<xacro:property name="base_correction" value="${pi}" />
<link name="${prefix}base_link" >
<visual>
<origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/base.dae"/>
</geometry>
......@@ -77,6 +68,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/base.stl"/>
</geometry>
......@@ -89,7 +81,7 @@
<joint name="${prefix}shoulder_pan_joint" type="revolute">
<parent link="${prefix}base_link" />
<child link = "${prefix}shoulder_link" />
<origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
<origin xyz="${__kinematics['shoulder']['x']} ${__kinematics['shoulder']['y']} ${__kinematics['shoulder']['z']}" rpy="${__kinematics['shoulder']['roll']} ${__kinematics['shoulder']['pitch']} ${__kinematics['shoulder']['yaw'] + base_correction}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
......@@ -102,6 +94,7 @@
<link name="${prefix}shoulder_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/shoulder.dae"/>
</geometry>
......@@ -110,6 +103,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/shoulder.stl"/>
</geometry>
......@@ -122,8 +116,8 @@
<joint name="${prefix}shoulder_lift_joint" type="revolute">
<parent link="${prefix}shoulder_link" />
<child link = "${prefix}upper_arm_link" />
<origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
<axis xyz="0 1 0" />
<origin xyz="${__kinematics['upper_arm']['x']} ${__kinematics['upper_arm']['y']} ${__kinematics['upper_arm']['z']}" rpy="${__kinematics['upper_arm']['roll']} ${__kinematics['upper_arm']['pitch']} ${__kinematics['upper_arm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
</xacro:unless>
......@@ -135,6 +129,7 @@
<link name="${prefix}upper_arm_link">
<visual>
<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/upperarm.dae"/>
</geometry>
......@@ -143,20 +138,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/upperarm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-a2}" mass="${upper_arm_mass}">
<origin xyz="0.0 0.0 ${-a2/2.0}" rpy="0 0 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}elbow_joint" type="revolute">
<parent link="${prefix}upper_arm_link" />
<child link = "${prefix}forearm_link" />
<origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
<axis xyz="0 1 0" />
<origin xyz="${__kinematics['forearm']['x']} ${__kinematics['forearm']['y']} ${__kinematics['forearm']['z']}" rpy="${__kinematics['forearm']['roll']} ${__kinematics['forearm']['pitch']} ${__kinematics['forearm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
</xacro:unless>
......@@ -168,7 +164,8 @@
<link name="${prefix}forearm_link">
<visual>
<geometry>
<origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/forearm.dae"/>
</geometry>
<material name="LightGrey">
......@@ -176,19 +173,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/forearm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-a3}" mass="${forearm_mass}">
<origin xyz="0.0 0.0 ${-a3/2.0}" rpy="0 0 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_1']['x']}" mass="${forearm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}wrist_1_joint" type="revolute">
<parent link="${prefix}forearm_link" />
<child link = "${prefix}wrist_1_link" />
<origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
<origin xyz="${__kinematics['wrist_1']['x']} ${__kinematics['wrist_1']['y']} ${__kinematics['wrist_1']['z']}" rpy="${__kinematics['wrist_1']['roll']} ${__kinematics['wrist_1']['pitch']} ${__kinematics['wrist_1']['yaw']}" />
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
......@@ -201,6 +199,7 @@
<link name="${prefix}wrist_1_link">
<visual>
<origin xyz="0 0 -0.104" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/wrist1.dae"/>
</geometry>
......@@ -209,19 +208,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.104" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/wrist1.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_1_mass}">
<origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}wrist_2_joint" type="revolute">
<parent link="${prefix}wrist_1_link" />
<child link = "${prefix}wrist_2_link" />
<origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
<origin xyz="${__kinematics['wrist_2']['x']} ${__kinematics['wrist_2']['y']} ${__kinematics['wrist_2']['z']}" rpy="${__kinematics['wrist_2']['roll']} ${__kinematics['wrist_2']['pitch']} ${__kinematics['wrist_2']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
......@@ -234,6 +234,7 @@
<link name="${prefix}wrist_2_link">
<visual>
<origin xyz="0 0 -0.08535" rpy="0 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/wrist2.dae"/>
</geometry>
......@@ -242,20 +243,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.08535" rpy="0 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/wrist2.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="0.12" mass="${wrist_2_mass}">
<origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" />
<xacro:cylinder_inertial radius="0.075" length="${__kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}wrist_3_joint" type="revolute">
<parent link="${prefix}wrist_2_link" />
<child link = "${prefix}wrist_3_link" />
<origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
<axis xyz="0 1 0" />
<origin xyz="${__kinematics['wrist_3']['x']} ${__kinematics['wrist_3']['y']} ${__kinematics['wrist_3']['z']}" rpy="${__kinematics['wrist_3']['roll']} ${__kinematics['wrist_3']['pitch']} ${__kinematics['wrist_3']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
</xacro:unless>
......@@ -267,6 +269,7 @@
<link name="${prefix}wrist_3_link">
<visual>
<origin xyz="0 0 -0.0921" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/visual/wrist3.dae"/>
</geometry>
......@@ -275,19 +278,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.0921" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur3e/collision/wrist3.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.032" length="0.042" mass="${wrist_3_mass}">
<origin xyz="0.0 ${wrist_3_length - 0.021} 0.0" rpy="${pi/2} 0 0" />
<xacro:cylinder_inertial radius="0.045" length="0.05" mass="${wrist_3_mass}">
<origin xyz="0.0 0.0 ${-0.05/2}" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}ee_fixed_joint" type="fixed">
<parent link="${prefix}wrist_3_link" />
<child link = "${prefix}ee_link" />
<origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 ${pi/2.0}" />
</joint>
<link name="${prefix}ee_link">
......@@ -309,7 +313,7 @@
not corrected wrt the real robot (ie: rotated over 180
degrees)
-->
<origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
<origin xyz="0 0 0" rpy="0 0 ${-base_correction}"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}base"/>
</joint>
......@@ -317,7 +321,7 @@
<!-- Frame coincident with all-zeros TCP on UR controller -->
<link name="${prefix}tool0"/>
<joint name="${prefix}wrist_3_link-tool0_fixed_joint" type="fixed">
<origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${prefix}wrist_3_link"/>
<child link="${prefix}tool0"/>
</joint>
......
......@@ -9,6 +9,7 @@
<xacro:include filename="$(find ur_e_description)/urdf/ur3e.urdf.xacro" />
<!-- arm -->
<xacro:arg name="kinematics_config" default="$(find ur_e_description)/config/ur3e_default.yaml"/>
<xacro:ur3e_robot prefix="" joint_limited="true"
shoulder_pan_lower_limit="${-pi}" shoulder_pan_upper_limit="${pi}"
shoulder_lift_lower_limit="${-pi}" shoulder_lift_upper_limit="${pi}"
......@@ -16,6 +17,7 @@
wrist_1_lower_limit="${-pi}" wrist_1_upper_limit="${pi}"
wrist_2_lower_limit="${-pi}" wrist_2_upper_limit="${pi}"
wrist_3_lower_limit="${-pi}" wrist_3_upper_limit="${pi}"
kinematics_file="${load_yaml('$(arg kinematics_config)')}"
/>
<link name="world" />
......
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="ur3e" >
name="ur3e" >
<!-- common stuff -->
<xacro:include filename="$(find ur_e_description)/urdf/common.gazebo.xacro" />
......@@ -9,7 +9,10 @@
<xacro:include filename="$(find ur_e_description)/urdf/ur3e.urdf.xacro" />
<!-- arm -->
<xacro:ur3e_robot prefix="" joint_limited="false"/>
<xacro:arg name="kinematics_config" default="$(find ur_e_description)/config/ur3e_default.yaml"/>
<xacro:ur3e_robot prefix="" joint_limited="false"
kinematics_file="${load_yaml('$(arg kinematics_config)')}"
/>
<link name="world" />
......
......@@ -15,12 +15,15 @@
</xacro:macro>
<xacro:macro name="ur5e_robot" params="prefix joint_limited
shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}">
shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
kinematics_file">
<xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
......@@ -38,44 +41,12 @@
<xacro:property name="wrist_2_cog" value="0.0 0.0018 0.11099" />
<xacro:property name="wrist_3_cog" value="0.0 0.001159 0.0" />
<!-- Kinematic model -->
<!-- Properties from urcontrol.conf -->
<!--
DH for ur5e:
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] ]
-->
<xacro:property name="d1" value="0.163" />
<xacro:property name="a2" value="-0.425" />
<xacro:property name="a3" value="-0.392" />
<xacro:property name="d4" value="0.127" />
<xacro:property name="d5" value="0.100" />
<xacro:property name="d6" value="0.100" />
<!-- Arbitrary offsets for shoulder/elbow joints -->
<xacro:property name="shoulder_offset" value="0.138" />
<xacro:property name="elbow_offset" value="-0.131" />
<xacro:property name="elbow_offset" value="0.007" />
<xacro:property name="upper_arm_inertia_offset" value="0.138" /> <!-- measured from model -->
<!-- link lengths used in model -->
<xacro:property name="shoulder_height" value="${d1}" />
<xacro:property name="upper_arm_length" value="${-a2}" />
<xacro:property name="forearm_length" value="${-a3}" />
<xacro:property name="wrist_1_length" value="${d4}" />
<xacro:property name="wrist_2_length" value="${d5}" />
<xacro:property name="wrist_3_length" value="${d6}" />
<!--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" /-->
<xacro:property name="shoulder_radius" value="0.060" /> <!-- manually measured -->
<xacro:property name="upper_arm_radius" value="0.054" /> <!-- manually measured -->
......@@ -83,9 +54,12 @@
<xacro:property name="forearm_radius" value="0.040" /> <!-- manually measured -->
<xacro:property name="wrist_radius" value="0.045" /> <!-- manually measured -->
<!-- This is for compatibility reasons, as transformation between base and base_link was always 180 degree in the ur_description -->
<xacro:property name="base_correction" value="${pi}" />
<link name="${prefix}base_link" >
<visual>
<origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/visual/base.dae" />
</geometry>
......@@ -94,6 +68,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/collision/base.stl" />
</geometry>
......@@ -106,7 +81,7 @@
<joint name="${prefix}shoulder_pan_joint" type="revolute">
<parent link="${prefix}base_link" />
<child link = "${prefix}shoulder_link" />
<origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
<origin xyz="${__kinematics['shoulder']['x']} ${__kinematics['shoulder']['y']} ${__kinematics['shoulder']['z']}" rpy="${__kinematics['shoulder']['roll']} ${__kinematics['shoulder']['pitch']} ${__kinematics['shoulder']['yaw'] + base_correction}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.14"/>
......@@ -119,6 +94,7 @@
<link name="${prefix}shoulder_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/visual/shoulder.dae" />
</geometry>
......@@ -127,6 +103,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/collision/shoulder.stl" />
</geometry>
......@@ -139,8 +116,8 @@
<joint name="${prefix}shoulder_lift_joint" type="revolute">
<parent link="${prefix}shoulder_link" />
<child link = "${prefix}upper_arm_link" />
<origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
<axis xyz="0 1 0" />
<origin xyz="${__kinematics['upper_arm']['x']} ${__kinematics['upper_arm']['y']} ${__kinematics['upper_arm']['z']}" rpy="${__kinematics['upper_arm']['roll']} ${__kinematics['upper_arm']['pitch']} ${__kinematics['upper_arm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.14"/>
</xacro:unless>
......@@ -152,6 +129,7 @@
<link name="${prefix}upper_arm_link">
<visual>
<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/visual/upperarm.dae" />
</geometry>
......@@ -160,20 +138,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/collision/upperarm.stl" />
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.06" length="${upper_arm_length}" mass="${upper_arm_mass}">
<origin xyz="0.0 0.0 ${upper_arm_length/2}" rpy="0 0 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>
<joint name="${prefix}elbow_joint" type="revolute">
<parent link="${prefix}upper_arm_link" />
<child link = "${prefix}forearm_link" />
<origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
<axis xyz="0 1 0" />
<origin xyz="${__kinematics['forearm']['x']} ${__kinematics['forearm']['y']} ${__kinematics['forearm']['z']}" rpy="${__kinematics['forearm']['roll']} ${__kinematics['forearm']['pitch']} ${__kinematics['forearm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
</xacro:unless>
......@@ -185,6 +164,7 @@
<link name="${prefix}forearm_link">
<visual>
<origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur5e/visual/forearm.dae" />
</geometry>
......@@ -193,19 +173,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>