Commit dede499a authored by Felix Mauch's avatar Felix Mauch
Browse files

use calibration in ur10e

parent 41bcf2c3
kinematics:
shoulder:
x: 0
y: 0
z: 0.1807
roll: -0
pitch: 0
yaw: -0
upper_arm:
x: 0
y: 0
z: 0
roll: 1.570796327
pitch: 0
yaw: -0
forearm:
x: -0.6127
y: 0
z: 0
roll: -0
pitch: 0
yaw: -0
wrist_1:
x: -0.57155
y: 0
z: 0.17415
roll: -0
pitch: 0
yaw: -0
wrist_2:
x: 0
y: -0.11985
z: -2.458164590756244e-11
roll: 1.570796327
pitch: 0
yaw: -0
wrist_3:
x: 0
y: 0.11655
z: -2.390480459346185e-11
roll: 1.570796326589793
pitch: 3.141592653589793
yaw: 3.141592653589793
<?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/ur10e_robot.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur10e_joint_limited_robot.urdf.xacro'" />
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
<param unless="$(arg limited)" name="robot_description"
command="$(find xacro)/xacro --inorder '$(find ur_e_description)/urdf/ur10e_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/ur10e_joint_limited_robot.urdf.xacro'
kinematics_config:=$(arg kinematics_config)"
/>
</launch>
......@@ -20,12 +20,15 @@
</xacro:macro>
<xacro:macro name="ur10e_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 -->
......@@ -47,29 +50,18 @@
<xacro:property name="wrist_2_cog" value="-0.00021 0.00112 0.002269" />
<xacro:property name="wrist_3_cog" value="0 -0.001156 -0.00149" />
<!-- Kinematic model -->
<!-- Properties from urcontrol.conf -->
<xacro:property name="d1" value="0.181" />
<xacro:property name="a2" value="-0.613" />
<xacro:property name="a3" value="-0.571" />
<xacro:property name="d4" value="0.135" />
<xacro:property name="d5" value="0.120" />
<xacro:property name="d6" value="0.117" />
<!-- Arbitrary offsets for shoulder/elbow joints -->
<xacro:property name="shoulder_offset" value="0.176" />
<xacro:property name="elbow_offset" value="-0.137" />
<xacro:property name="shoulder_offset" value="0.1762" /> <!-- measured from model -->
<xacro:property name="elbow_offset" value="0.0393" /> <!-- measured from model -->
<xacro:property name="upper_arm_inertia_offset" value="0.175" /> <!-- 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/ur10e/visual/base.dae"/>
</geometry>
......@@ -78,6 +70,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${base_correction}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/collision/base.stl"/>
</geometry>
......@@ -90,7 +83,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"/>
......@@ -103,6 +96,7 @@
<link name="${prefix}shoulder_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/visual/shoulder.dae"/>
</geometry>
......@@ -111,6 +105,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/collision/shoulder.stl"/>
</geometry>
......@@ -123,8 +118,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>
......@@ -136,6 +131,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/ur10e/visual/upperarm.dae"/>
</geometry>
......@@ -144,20 +140,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/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>
......@@ -169,6 +166,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/ur10e/visual/forearm.dae"/>
</geometry>
......@@ -177,20 +175,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/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" />
<axis xyz="0 1 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 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
</xacro:unless>
......@@ -202,6 +201,7 @@
<link name="${prefix}wrist_1_link">
<visual>
<origin xyz="0 0 -0.135" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist1.dae"/>
</geometry>
......@@ -210,19 +210,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.135" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/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"/>
......@@ -235,6 +236,7 @@
<link name="${prefix}wrist_2_link">
<visual>
<origin xyz="0 0 -0.12" rpy="0 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist2.dae"/>
</geometry>
......@@ -243,20 +245,21 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.12" rpy="0 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/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>
......@@ -268,6 +271,7 @@
<link name="${prefix}wrist_3_link">
<visual>
<origin xyz="0 0 -0.1168" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/visual/wrist3.dae"/>
</geometry>
......@@ -276,19 +280,20 @@
</material>
</visual>
<collision>
<origin xyz="0 0 -0.1168" rpy="${pi/2} 0 0"/>
<geometry>
<mesh filename="package://ur_e_description/meshes/ur10e/collision/wrist3.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.045" length="0.05" mass="${wrist_3_mass}">
<origin xyz="0.0 ${wrist_3_length - 0.05/2} 0.0" rpy="${pi/2} 0 0" />
<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">
......@@ -310,7 +315,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>
......@@ -318,7 +323,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,13 +9,15 @@
<xacro:include filename="$(find ur_e_description)/urdf/ur10e.urdf.xacro" />
<!-- arm -->
<xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
<xacro:ur10e_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}"
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="${load_yaml('$(arg kinematics_config)')}"
/>
<link name="world" />
......
......@@ -9,7 +9,10 @@
<xacro:include filename="$(find ur_e_description)/urdf/ur10e.urdf.xacro" />
<!-- arm -->
<xacro:ur10e_robot prefix="" joint_limited="false"/>
<xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
<xacro:ur10e_robot prefix="" joint_limited="false"
kinematics_file="${load_yaml('$(arg kinematics_config)')}"
/>
<link name="world" />
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment