Apriltags+Ros+Ur5项目–universal_robot

153
0
2021年1月22日 09时15分

×××××××××××××××××××××××××××××××××××××
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}" />
    <xacro:property name="forearm_length" value="${-a3}" />
    <xacro:property name="wrist_1_length" value="${d4 - elbow_offset - shoulder_offset}" />
    <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 -->
    <xacro:property name="elbow_radius" value="0.060" />      <!-- manually measured -->
    <xacro:property name="forearm_radius" value="0.040" />    <!-- manually measured -->
    <xacro:property name="wrist_radius" value="0.045" />      <!-- manually measured -->


    <link name="${prefix}base_link" >
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/base.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/base.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.05" mass="${base_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <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" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}shoulder_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/shoulder.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.15" mass="${shoulder_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <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" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}upper_arm_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/upperarm.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.56" mass="${upper_arm_mass}">
        <origin xyz="0.0 0.0 0.28" rpy="0 0 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" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}forearm_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/forearm.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.5" mass="${forearm_mass}">
        <origin xyz="0.0 0.0 0.25" rpy="0 0 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" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_1_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist1.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" 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" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_2_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" 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" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_3_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_3_mass}">
        <origin xyz="0.0 0.0 0.0" 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}" />
    </joint>

    <link name="${prefix}ee_link">
      <collision>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <origin rpy="0 0 0" xyz="-0.01 0 0"/>
      </collision>
    </link>

   

    <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />
    <xacro:ur_arm_gazebo prefix="${prefix}" />

    <!-- ROS base_link to UR 'Base' Coordinates transform -->
    <link name="${prefix}base"/>
    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
      <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
      <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 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
      <parent link="${prefix}wrist_3_link"/>
      <child link="${prefix}tool0"/>
    </joint>




   <link name="camera"/>
    <joint name="$base_link_to_camera" type="fixed">
      <parent link="${prefix}base"/>
      <child link="camera"/>
      <!--origin xyz="-0.0109527 -0.7819547 1.1120832" rpy="3.1415926 0 0"/ -->
      <!--origin xyz="-0.0009527 -0.7819547 1.1120832" rpy="3.1415926 0 0"/-->
      <!--origin xyz="0.0259527 -0.7819547 1.1120832" rpy="3.1415926 0 0"/-->
      <!--origin xyz="0.0182949 -1.25177516 0.3989193" rpy="-1.5707963 0 0"/-->
      <!--origin xyz="0.0591778 -1.25177516 0.3589193" rpy="-1.5707963 0 0"/-->
      <!--origin xyz="0.0291778 -1.20677516 0.5589193" rpy="-1.5707963 0 0"/-->
      <origin xyz="-0.0691778 1.15077516 0.5089193" rpy="-1.5150 0.0160 -3.0716"/>
      <!--origin xyz="-0.0491778 1.18677516 0.4589193" rpy="-1.5150 0.0160 -3.0716"/zuihou-->
      <!--origin xyz="0.0291778 1.20677516 0.5589193" rpy="1.5707963 0 0"/-->
    </joint>

 <link name="table">
    <visual>
      <geometry>
        <box size="0.8 0.6 0.05"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="1.5 1.5 0.05"/>
      </geometry>
    </collision>
  </link>
 <joint name="table_to_robot" type="fixed">
    <parent link="${prefix}base_link"/>
    <child link="table"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

 <link name="wall_left">
    <visual>
      <geometry>
        <box size="0.01 0.6 1.2"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.01 0.6 1.2"/>
      </geometry>
    </collision>
  </link>
 <joint name="wall_left_to_robot" type="fixed">
    <parent link="${prefix}base_link"/>
    <child link="wall_left"/>
    <origin xyz="-0.4 0 0.6" rpy="0 0 0"/>
  </joint>

 <!--link name="wall_right">
    <visual>
      <geometry>
        <box size="0.01 0.6 1.2"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.01 0.6 1.2"/>
      </geometry>
    </collision>
  </link>
 <joint name="wall_right_to_robot" type="fixed">
    <parent link="${prefix}base_link"/>
    <child link="wall_right"/>
    <origin xyz="0.4 0 0.6" rpy="0 0 0"/>
  </joint-->

 <link name="wall_behind">
    <visual>
      <geometry>
        <box size="0.01 0.8 1.2"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.01 0.6 1.2"/>
      </geometry>
    </collision>
  </link>
 <joint name="wall_behind_to_robot" type="fixed">
    <parent link="${prefix}base_link"/>
    <child link="wall_behind"/>
    <origin xyz="0 0.3 0.6" rpy="0 0 ${pi/2.0}"/>
  </joint>

 <link name="wall_up">
    <visual>
      <geometry>
        <box size="0.8 0.6 0.01"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.8 0.6 0.01"/>
      </geometry>
    </collision>
  </link>
 <joint name="wall_up_to_robot" type="fixed">
    <parent link="${prefix}base_link"/>
    <child link="wall_up"/>
    <origin xyz="0 0 1.2" rpy="0 0 0"/>
  </joint>





  </xacro:macro>
</robot>

 

这个是在官方提供的代码的基础上进行的修改,关于UR5-配置MoveIt!运动学插件IKFAST,参考以下网址:https://blog.csdn.net/harrycomeon/article/details/95517129

发表评论

后才能评论