Apriltags+Ros+Ur5项目--universal_robot

×××××××××××××××××××××××××××××××××××××
8.15号更新
新的github链接：https://github.com/harrycomeon/Project1-Apriltags-UR5

由于直接采用myrobot的效果并不好，所以选择在universal_robot文件下对ur_description的urdf文件下的ur5.urdf.xacro进行修改，修改如下，添加了标定后的相机的姿态以及添加了几个墙面作为障碍物，从而约束规划路径。代码如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur5">
<!--robot xmlns:xacro="http://ros.org/wiki/xacro"name="ur5"-->

  <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" />

  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
    <inertial>
      <mass value="${mass}" />
      <xacro:insert_block name="origin" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
        izz="${0.5 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="ur5_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}
    transmission_hw_interface:=hardware_interface/PositionJointInterface">

    <!-- Inertia parameters -->
    <xacro:property name="base_mass" value="4.0" />  <!-- This mass might be incorrect -->
    <xacro:property name="shoulder_mass" value="3.7000" />
    <xacro:property name="upper_arm_mass" value="8.3930" />
    <xacro:property name="forearm_mass" value="2.2750" />
    <xacro:property name="wrist_1_mass" value="1.2190" />
    <xacro:property name="wrist_2_mass" value="1.2190" />
    <xacro:property name="wrist_3_mass" value="0.1879" />

    <xacro:property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
    <xacro:property name="upper_arm_cog" value="0.0 -0.024201 0.2125" />
    <xacro:property name="forearm_cog" value="0.0 0.0265 0.11993" />
    <xacro:property name="wrist_1_cog" value="0.0 0.110949 0.01634" />
    <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 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] ]
    -->
    <xacro:property name="d1" value="0.089159" />
    <xacro:property name="a2" value="-0.42500" />
    <xacro:property name="a3" value="-0.39225" />
    <xacro:property name="d4" value="0.10915" />
    <xacro:property name="d5" value="0.09465" />
    <xacro:property name="d6" value="0.0823" />

    <!-- Arbitrary offsets for shoulder/elbow joints -->
    <xacro:property name="shoulder_offset" value="0.13585" />  <!-- measured from model -->
    <xacro:property name="elbow_offset" value="-0.1197" /> <!-- measured from model -->

    <!-- link lengths used in model -->
    <xacro:property name="shoulder_height" value="${d1}" />
    <xacro:property name="upper_arm_length" value="${-a2}" />