@小伊 <?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://ros.org/wiki/xacro"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find kr5_description)/urdf/kr.transmission.xacro" />
<xacro:include filename="$(find kr5_description)/urdf/kr.gazebo.xacro" />
<!-- effort values can be found here: https://github.com/ros-industrial/universal_robot/issues/32-->
<!-- plate Properties -->
<!-- a plate with 40x40x1 (cm) -->
<xacro:property name="plate_mass" value="0.5175" />
<xacro:property name="plate_width" value="0.4" />
<!-- Kinematic model -->
<xacro:macro name="kr5_robot" params="prefix">
<!-- links: -->
<link name="${prefix}base_link" >
<inertial>
<origin xyz="-0.017519394031846 8.11394768450848E-07 0.0975924044854325" rpy="0 0 0" />
<mass value="7.58585707666653" />
<!-- changed mass! 7000? -->
<inertia ixx="0.0441803207364147" ixy="5.37433593375545E-07" ixz="0.004045564920652" iyy="0.0584309411563095" iyz="-4.8990117845347E-07" izz="0.0514875586171667" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/base_link.dae" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/base_link.dae" />
</geometry>
</collision>
</link>
<link name="${prefix}link_1">
<inertial>
<origin xyz="0.0359968408135882 0.00783796591813503 -0.0864969937502713" rpy="0 0 0" />
<mass value="5.77242340173648" />
<inertia ixx="0.0327019582662897" ixy="-0.00189987845379704" ixz="0.00937014136132958" iyy="0.0373787564144547" iyz="-0.00197023031658988" izz="0.0348614383349291" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/link_1.dae" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_1.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_2">
<inertial>
<origin xyz="0.00610231661046294 0.0401115026555897 -0.11531122862673" rpy="0 0 0" />
<mass value="4.64974639735464" />
<inertia ixx="0.0663459530931455" ixy="0.000173652478770081" ixz="0.000586191290902105" iyy="0.070407913174161" iyz="-0.00203071071007864" izz="0.0102816377550493" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/link_2.dae" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_2.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_3">
<inertial>
<origin xyz="0.0755031858462784 -0.046481517407519 -0.00506472105904233" rpy="0 0 0" />
<mass value="4.3593398708568" />
<inertia ixx="0.0202008069207718" ixy="5.72394748332304E-05" ixz="0.000556621203065716" iyy="0.0246796752429931" iyz="0.000697020296811431" izz="0.0178067561136726" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/link_3.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_3.stl" />
</geometry>
</collision>
</link>
<!-- wrist links: -->
<link name="${prefix}link_4">
<inertial>
<origin xyz="0.0857493724873204 -0.00149370513017266 -0.00271609825446984" rpy="0 0 0" />
<mass value="2.06809715541017" />
<inertia ixx="0.00444943757623485" ixy="-0.000424915429214687" ixz="-0.000117340671961173" iyy="0.00882693069799353" iyz="0.000136971152627904" izz="0.0101131228921778" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/link_4.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_4.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_5">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.328309098974388" />
<inertia ixx="0.000221789219477276" ixy="-9.59628913869705E-07" ixz="1.40395663663479E-20" iyy="0.000321152451627032" iyz="-4.44973606670705E-20" izz="0.000275966419870925" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_5.stl" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_5.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}link_6">
<inertial>
<origin xyz="0.00983949894991115 -2.78372724487979E-05 -1.11022302462516E-15" rpy="0 0 0" />
<mass value="0.0789494717998531" />
<inertia ixx="3.53285039019399E-05" ixy="-3.23299732456798E-08" ixz="-6.9643163510009E-19" iyy="2.30177957763861E-05" iyz="-9.26550270485857E-19" izz="2.28539400944714E-05" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/visual/link_6.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/collision/link_6.stl" />
</geometry>
</collision>
</link>
<!-- tool link -->
<!-- This frame corresponds to the $FLANGE coordinate system in KUKA KRC controllers. -->
<link name="${prefix}ee_link">
<inertial>
<origin xyz="0 -.01 0.17275327" rpy="0 0 0" />
<mass value="0.085" />
<inertia ixx="0.002858" ixy="0" ixz="0" iyy="0.002987" iyz="0" izz="0.0001420" /> <!-- ${0.08333333333*plate_mass*plate_width}-->
</inertial>
<visual>
<origin xyz="0.075 -0.012 0.06" rpy="0 ${pi} 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/tools/paddle.stl" scale="0.001 0.001 0.001" /> <!-- plate is 20x20x1cm if scale is 0.01 -->
</geometry>
</visual>
<collision>
<origin xyz="0.075 -0.012 0.06" rpy="0 ${pi} 0" />
<geometry>
<mesh filename="package://kr5_description/meshes/tools/paddle.stl" scale="0.001 0.001 0.001" /> <!-- plate is 20x20x1 m -->
</geometry>
<surface>
<bounce restitution_coefficient="0.1" threshold="0.01"/>
</surface>
</collision>
</link>
<gazebo reference="${prefix}ee_link">
<mu1 value="100.0" />
<mu2 value="0.1" />
<maxVel value="10.0"/>
<kp value="1000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0" />
<material>Gazebo/Grey</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- joints: -->
<joint name="${prefix}joint_a1" type="revolute">
<origin xyz="0 0 0.4" rpy="${pi} 0 0" />
<parent link="${prefix}base_link" />
<child link = "${prefix}link_1" />
<axis xyz="0 0 1" />
<limit effort="150" lower="${radians(-155)}" upper="${radians(155)}" velocity="${radians(15400)}" />
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}joint_a2" type="revolute">
<origin xyz="0.18 0 0" rpy="${pi/2} 0.0 0" />
<parent link="${prefix}link_1" />
<child link = "${prefix}link_2" />
<axis xyz="0 0 1" />
<limit lower="${radians(-180)}" upper="${radians(65)}" effort="150.0" velocity="${radians(15400)}"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}joint_a3" type="revolute">
<parent link="${prefix}link_2" />
<child link = "${prefix}link_3" />
<origin xyz="0.6 0 0" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit lower="${radians(-15)}" upper="${radians(158)}" effort="150.0" velocity="315"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}joint_a4" type="revolute">
<parent link="${prefix}link_3" />
<child link = "${prefix}link_4" />
<origin xyz="0 -0.12 0" rpy="0 ${-pi/2} 0" />
<axis xyz="0 0 1" />
<limit lower="${radians(-350)}" upper="${radians(350)}" effort="60.0" velocity="320"/> <!-- max should be 28 -->
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}joint_a5" type="revolute">
<parent link="${prefix}link_4" />
<child link = "${prefix}link_5" />
<origin xyz="0 0 -0.62" rpy="0 ${pi/2} 0" />
<axis xyz="0 0 1" />
<limit lower="${radians(-130)}" upper="${radians(130)}" effort="60.0" velocity="320"/> <!-- max should be 28 -->
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}joint_a6" type="revolute">
<parent link="${prefix}link_5" />
<child link = "${prefix}link_6" />
<origin xyz="0 0 0" rpy="0 ${-pi/2} 0" />
<axis xyz="0 0 1" />
<limit lower="${radians(-350)}" upper="${radians(350)}" effort="28.0" velocity="320"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="${prefix}ee_fixed_joint" type="fixed">
<parent link="${prefix}link_6" />
<child link = "${prefix}ee_link" />
<origin xyz="0 0 -.308" rpy="0 ${-pi} ${-pi/2}" />
</joint>
<!-- -.368 is 0 visual offset -->
<xacro:kr_arm_transmission prefix="${prefix}" />
<xacro:kr_arm_gazebo prefix="${prefix}" />
<!-- ROS base_link to UR 'Base' Coordinates transform -->
<!-- NOTE: this rotation is only needed as long as base_link itself is
not corrected wrt the real robot (ie: rotated over 180
degrees)
-->
<!-- <link name="${prefix}base"/>
<joint name="${prefix}base_link-base_fixed_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}base"/>
</joint> -->
<!-- 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 0.115 0" rpy="${pi/-2.0} 0 0"/>
<parent link="${prefix}link_6"/>
<child link="${prefix}tool0"/>
</joint> -->
</xacro:macro>
</robot>
这个是xacro文件