Lesson 19:moveit-urdf description file

从这一节开始我们讲述如何使用moveit来控制机械臂,moveit是开源运动规划库OMPL的ROS接口,其封装了OMPL的复杂功能,是的用户可以通过相对简单的配置,就可以在ROS中使用OMPL先进的控制算法,下面我们就开始moveit的第一节内容,创建Diego 1# plus的urdf描述文件,有关urdf的规范,请参考官方ROS wiki

Diego 1# plus完成后的urdf源代码请参阅github,在rviz中的显示效果如下:

您可以执行如下命令打开diego1.urdf,注意要首先进入diego1.urdf所在的目录,或者命令中包含完整的目录路径

roslaunch urdf_tutorial display.launch model:=diego1.urdf

Diego 1# plus的模型主要包括三个部分:

  • 底盘
  • 控制单元及摄像头:摄像头放置的比较高的原因是xtion的可视范围是0.8m~3m,如果放置的比较低的话,机械臂的运动范围就不在其可视范围之内。
  • 机械臂部分

1.urdf的基本语法

1.1 urdf的结构

下图是官方教程中的截图,整个urdf文件就是由link和joint组成。

  • link是指机器人的一个部件,比如说机器人的底盘,摄像头,手臂等。
  • Joint是指机器人的各个部件的连接关节。

1.2 robot节点

urdf本身是一个xml文件,遵循xml version 1.0规范。urdf的最高层的节点是<robot>,在这个节点中我们可以定义机器人的名称

<?xml version="1.0"?>
<robot name="diego1">

...

</robot>

1.3Link节点

我们拿diego1.urdf中的部分代码来做解释,下面一段代码是diego1.urdf中控制器的描述,其实就是mini pc加上arduino,电源模块,I2c舵机控制板,在diego1.urdf中我们把这一部分整体描述成为一个矩形组件。

<link name="control_box">
   <visual>
   <geometry>
      <box size="0.16 .128 .14"/>
   </geometry>
   <material name="black">
      <color rgba="255 255 255 1"/>
   </material>
   </visual>
</link>

下面我们来解释这段代码

<link name="control_box"><!--link节点的标识,这里需要给每个节点命名一个唯一的名称-->
   <visual><!--节点视觉描述部分-->
   <geometry><!--描述节点外观尺寸部分-->
      <box size="0.16 .128 .14"/><!--box说明此节点的形状为矩形,长宽高分别为0.16m,0.128m,1.14m-->
   </geometry>
   <material name="black"><!--节点外观颜色,文字描述部分,这里命名为black-->
      <color rgba="255 255 255 1"/><!--这里描述了节点的颜色,采用的rgba色彩系-->
   </material>
   </visual>
</link>

link节点还可以是圆柱体,和球状体,如下代码是diego1.urdf中车轮的描述部分

<link name="tyer_master_right_motor">
  <visual>
  <geometry>
     <cylinder length=".068" radius="0.0075"></cylinder>
  </geometry>
  <material name="black">
     <color rgba="0 0 0 1"/>
  </material> 
  </visual>
</link>

这段代码中的

<cylinder length=".068" radius="0.0075"></cylinder>

把此节点描述成为一个半径为0.0075m,而高为0.068m的圆柱体

球状的link语法和矩形,圆柱体基本类似,用的关键词是<sphere>,在Diego1.urdf中没有用到。

1.4 Joint节点

Joint节点用来标识两个link之间的连接关系,本质上是两个link之间坐标关系的转换。下面是control_box和base_link之间的Joint

<joint name="base_to_control_box" type="fixed">
   <parent link="base_link"/>
   <child link="control_box"/>
   <origin xyz="-0.07 0.0 0.0715"/>
</joint>

解释如下

<joint name="base_to_control_box" type="fixed"><!--joint节点名称,type="fixed"表明是固定的连接关系-->
   <parent link="base_link"/><!--父节点为base_link-->
   <child link="control_box"/><!--子节点为control_link-->
   <origin xyz="-0.07 0.0 0.0715"/><!--在父节点坐标系内x轴移动-0.07m,y轴移动0.0m,z轴移动0.0715m-->
</joint>

如下tyer_master_right_motor和节点right_leg之间的joint

<joint name="right_leg_to_master_right_motor" type="fixed">
   <axis xyz="0 0 1"/> 
   <parent link="right_leg"/>
   <child link="tyer_master_right_motor"/>
   <origin rpy="1.57075 0 0" xyz="0.03 0.0315 0.0175"/>
   <limit effort="100" velocity="100"/> 
   <joint_properties damping="0.0" friction="0.0"/>
</joint>

解释如下:

<joint name="right_leg_to_master_right_motor" type="fixed"><!--joint节点名称,type="continuous"表明是可以持续旋转的-->
   <axis xyz="0 0 1"/><!--旋转轴为z轴-->
   <parent link="right_leg"/><!--父节点为right_leg-->
   <child link="tyer_master_right_motor"/><!--子节点为tyer_master_right_motor-->
   <origin rpy="1.57075 0 0" xyz="0.03 0.0315 0.0175"/><!--子节点在父节点坐标系中在x轴旋转90度,x轴移动0.03m,y轴移动0.0315m,z轴移动0.0175m-->
   <limit effort="100" velocity="100"/><!--最大力度限制为100N-m,最大速度限制为100m/s-->
   <joint_properties damping="0.0" friction="0.0"/><!--指定damping为0,friction为0-->
</joint>

2.Diego urdf

urdf的制作我们还可以是用专业的3D建模工具来制作,完成后导出即可,但在diego1 #plus中我们使用基本的urdf元素来描述,虽然不能精确完整的描述urdf,但相对来说简单,对于初学者也比较容易理解,下面是diego1# plus完整的描述文件:

<?xml version="1.0"?>
<robot name="diego1">
<link name="base_link">
<visual>
<geometry>
<box size="0.20 .15 .003"/>
</geometry>
</visual>
</link>

<link name="left_leg">
<visual>
<geometry>
<box size="0.14 .003 .095"/>
</geometry>
</visual>
</link>

<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="0.0 0.075 -0.046"/>
</joint>

<link name="right_leg">
<visual>
<geometry>
<box size="0.14 .003 .095"/>
</geometry>
</visual>
</link>

<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0.0 -0.075 -0.046"/>
</joint>

<link name="left_leg_front">
<visual>
<geometry>
<box size="0.05 .003 .03"/>
</geometry>
</visual>
</link>

<joint name="base_to_left_leg_front" type="fixed">
<parent link="left_leg"/>
<child link="left_leg_front"/>
<origin xyz="0.095 0.0 -0.0025"/>
</joint>

<link name="left_leg_back">
<visual>
<geometry>
<box size="0.05 .003 .03"/>
</geometry>
</visual>
</link>

<joint name="base_to_left_leg_back" type="fixed">
<parent link="left_leg"/>
<child link="left_leg_back"/>
<origin xyz="-0.095 0.0 -0.0325"/>
</joint>

<link name="right_leg_front">
<visual>
<geometry>
<box size="0.05 .003 .03"/>
</geometry>
</visual>
</link>

<joint name="base_to_right_leg_front" type="fixed">
<parent link="right_leg"/>
<child link="right_leg_front"/>
<origin xyz="0.095 0.0 -0.0025"/>
</joint>

<link name="right_leg_back">
<visual>
<geometry>
<box size="0.05 .003 .03"/>
</geometry>
</visual>
</link>

<joint name="base_to_right_leg_back" type="fixed">
<parent link="right_leg"/>
<child link="right_leg_back"/>
<origin xyz="-0.095 0.0 -0.0325"/>
</joint>

<link name="tyer_master_right_axis">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_master_right_axix" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg"/>
<child link="tyer_master_right_axis"/>
<origin rpy="1.57075 0 0" xyz="0.03 -0.0315 0.0175"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_master_left_axis">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="left_leg_to_master_left_axis" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg"/>
<child link="tyer_master_left_axis"/>
<origin rpy="1.57075 0 0" xyz="0.03 0.0315 0.0175"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_master_right_motor">
<visual>
<geometry>
<cylinder length=".068" radius="0.0075"></cylinder>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_master_right_motor" type="fixed">
<axis xyz="0 0 1"/>
<parent link="right_leg"/>
<child link="tyer_master_right_motor"/>
<origin rpy="1.57075 0 0" xyz="0.03 0.0315 0.0175"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_master_left_motor">
<visual>
<geometry>
<cylinder length=".06" radius="0.0075"></cylinder>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="left_leg_to_master_left_motor" type="fixed">
<axis xyz="0 0 1"/>
<parent link="left_leg"/>
<child link="tyer_master_left_motor"/>
<origin rpy="1.57075 0 0" xyz="0.03 -0.0315 0.0175"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_right_axis1">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_right_axix1" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg_front"/>
<child link="tyer_slave_right_axis1"/>
<origin rpy="1.57075 0 0" xyz="0.02 -0.0315 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_right_axis2">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_right_axix2" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg"/>
<child link="tyer_slave_right_axis2"/>
<origin rpy="1.57075 0 0" xyz="0.063 -0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_right_axis3">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_right_axix3" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg"/>
<child link="tyer_slave_right_axis3"/>
<origin rpy="1.57075 0 0" xyz="0.008 -0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_right_axis4">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_right_axix4" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg"/>
<child link="tyer_slave_right_axis4"/>
<origin rpy="1.57075 0 0" xyz="-0.047 -0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_right_axis5">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_right_axix5" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_leg_back"/>
<child link="tyer_slave_right_axis5"/>
<origin rpy="1.57075 0 0" xyz="-0.018 -0.0315 -0.008"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_left_axis1">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_left_axix1" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg_front"/>
<child link="tyer_slave_left_axis1"/>
<origin rpy="1.57075 0 0" xyz="0.02 0.0315 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_left_axis2">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_left_axix2" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg"/>
<child link="tyer_slave_left_axis2"/>
<origin rpy="1.57075 0 0" xyz="0.063 0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_left_axis3">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_left_axix3" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg"/>
<child link="tyer_slave_left_axis3"/>
<origin rpy="1.57075 0 0" xyz="0.008 0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_left_axis4">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_left_axix4" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg"/>
<child link="tyer_slave_left_axis4"/>
<origin rpy="1.57075 0 0" xyz="-0.047 0.0315 -0.0405"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave_left_axis5">
<visual>
<geometry>
<cylinder length=".06" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave_left_axix5" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_leg_back"/>
<child link="tyer_slave_left_axis5"/>
<origin rpy="1.57075 0 0" xyz="-0.018 0.0315 -0.008"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_master_left">
<visual>
<geometry>
<cylinder length=".03" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_master_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_master_left_axis"/>
<child link="tyer_master_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_master_right">
<visual>
<geometry>
<cylinder length=".03" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_master_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_master_right_axis"/>
<child link="tyer_master_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave1_left">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave1_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_left_axis1"/>
<child link="tyer_slave1_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave2_left">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave2_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_left_axis2"/>
<child link="tyer_slave2_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave3_left">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave3_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_left_axis3"/>
<child link="tyer_slave3_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave4_left">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave4_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_left_axis4"/>
<child link="tyer_slave4_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave5_left">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave5_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_left_axis5"/>
<child link="tyer_slave5_left"/>
<origin rpy="0 0 0" xyz="0.0 0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave1_right">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave1_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_right_axis1"/>
<child link="tyer_slave1_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave2_right">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave2_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_right_axis2"/>
<child link="tyer_slave2_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>


<link name="tyer_slave3_right">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave3_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_right_axis3"/>
<child link="tyer_slave3_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave4_right">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave4_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_right_axis4"/>
<child link="tyer_slave4_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="tyer_slave5_right">
<visual>
<geometry>
<cylinder length=".024" radius="0.0275"></cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
</link>

<joint name="right_leg_to_slave5_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="tyer_slave_right_axis5"/>
<child link="tyer_slave5_right"/>
<origin rpy="0 0 0" xyz="0.0 -0.0025 0.00"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_left1">
<visual>
<geometry>
<box size="0.10 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_left1" type="fixed">
<parent link="tyer_master_left_motor"/>
<child link="caterpilar_left1"/>
<origin rpy="1.57075 0 -0.24178" xyz="0.05 0.02 -0.068"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_left2">
<visual>
<geometry>
<box size="0.16 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_left2" type="fixed">
<parent link="tyer_master_left_motor"/>
<child link="caterpilar_left2"/>
<origin rpy="1.57075 0 0.38178" xyz="-0.080 -0.00 -0.068"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_left3">
<visual>
<geometry>
<box size="0.07 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_left3" type="fixed">
<parent link="tyer_slave2_left"/>
<child link="caterpilar_left3"/>
<origin rpy="1.57075 0 0.64178" xyz="0.0405 -0.0065 -0.002"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_left4">
<visual>
<geometry>
<box size="0.18 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_left4" type="fixed">
<parent link="tyer_slave2_left"/>
<child link="caterpilar_left4"/>
<origin rpy="1.57075 0 0" xyz="-0.09 -0.028 -0.002"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_right1">
<visual>
<geometry>
<box size="0.10 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_right1" type="fixed">
<parent link="tyer_master_right_motor"/>
<child link="caterpilar_right1"/>
<origin rpy="1.57075 0 -0.24178" xyz="0.05 0.02 0.068"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_right2">
<visual>
<geometry>
<box size="0.16 .045 .0015"/>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_right2" type="fixed">
<parent link="tyer_master_right_motor"/>
<child link="caterpilar_right2"/>
<origin rpy="1.57075 0 0.38178" xyz="-0.080 -0.00 0.068"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_right3">
<visual>
<geometry>
<box size="0.07 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_right3" type="fixed">
<parent link="tyer_slave2_right"/>
<child link="caterpilar_right3"/>
<origin rpy="1.57075 0 0.64178" xyz="0.0405 -0.0065 0.002"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="caterpilar_right4">
<visual>
<geometry>
<box size="0.18 .045 .0015"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>

<joint name="tyer_master_left_motor_to_caterpilar_right4" type="fixed">
<parent link="tyer_slave2_right"/>
<child link="caterpilar_right4"/>
<origin rpy="1.57075 0 0" xyz="-0.09 -0.028 0.002"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="arm_base">
<visual>
<geometry>
<box size="0.11 .11 .013"/>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>

<joint name="base_to_arm_base" type="fixed">
<parent link="base_link"/>
<child link="arm_base"/>
<origin xyz="0.085 0.0 0.008"/>
</joint>

<link name="control_box">
<visual>
<geometry>
<box size="0.16 .128 .14"/>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>

<joint name="base_to_control_box" type="fixed">
<parent link="base_link"/>
<child link="control_box"/>
<origin xyz="-0.07 0.0 0.0715"/>
</joint>
<!-- camera holder-->
<link name="camera_holder">
<visual>
<geometry>
<box size="0.043 .03 .59"/>
</geometry>
<material name="silver">
<color rgba="161 161 161 1"/>
</material>
</visual>
</link>

<joint name="control_box_to_camera_holder" type="fixed">
<parent link="control_box"/>
<child link="camera_holder"/>
<origin xyz="-0.0215 0.0 0.365"/>
</joint>

<!-- xtion-->
<link name="xtion_camera_base">
<visual>
<geometry>
<box size="0.038 .10 .025"/>
</geometry>
<material name="black">
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>

<joint name="camera_holder_to_xtion_camera_base" type="fixed">
<parent link="camera_holder"/>
<child link="xtion_camera_base"/>
<origin xyz="0.0 0.0 0.3075"/>
</joint>

<link name="xtion_camera_base_neck">
<visual>
<geometry>
<box size="0.020 .020 .020"/>
</geometry>
<material name="black">
<color rgba="0 0 50 1"/>
</material>
</visual>
</link>

<joint name="xtion_camera_base_to_neck" type="fixed">
<parent link="xtion_camera_base"/>
<child link="xtion_camera_base_neck"/>
<origin xyz="0.009 0.0 0.0125"/>
</joint>

<link name="xtion_camera">
<visual>
<geometry>
<box size="0.038 .18 .027"/>
</geometry>
<material name="black">
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>

<joint name="xtion_camera_base_neck_to_camera" type="fixed">
<parent link="xtion_camera_base_neck"/>
<child link="xtion_camera"/>
<origin xyz="0.0 0.0 0.0235"/>
</joint>

<link name="xtion_camera_eye_left">
<visual>
<geometry>
<cylinder length=".002" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="xtion_camera_to_eye_left" type="fixed">
<axis xyz="0 0 1"/>
<parent link="xtion_camera"/>
<child link="xtion_camera_eye_left"/>
<origin rpy="0 1.57075 0" xyz="0.0191 0.045 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="xtion_camera_eye_right">
<visual>
<geometry>
<cylinder length=".002" radius="0.005"></cylinder>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
</link>

<joint name="xtion_camera_to_eye_right" type="fixed">
<axis xyz="0 0 1"/>
<parent link="xtion_camera"/>
<child link="xtion_camera_eye_right"/>
<origin rpy="0 1.57075 0" xyz="0.0191 -0.045 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!--arm-->
<link name="arm_round_joint_stevo0">
<visual>
<geometry>
<cylinder length=".013" radius="0.05"></cylinder>
</geometry>
<material name="blue">
<color rgba="0 0 255 1"/>
</material>
</visual>
</link>

<joint name="arm_base_to_arm_round_joint_stevo0" type="revolute">
<axis xyz="0 0 1"/>
<parent link="arm_base"/>
<child link="arm_round_joint_stevo0"/>
<origin rpy="0 0 0" xyz="0.00 0.00 0.013"/>
<limit effort="100" velocity="100" lower="-1.5707963" upper="1.5707963"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="shoulder_1">
<visual>
<geometry>
<box size="0.065 .065 .037"/>
</geometry>
<material name="silver1">
<color rgba="232 232 232 1"/>
</material>
</visual>
</link>

<joint name="arm_round_joint_stevo0_to_shoulder_1" type="fixed">
<parent link="arm_round_joint_stevo0"/>
<child link="shoulder_1"/>
<origin xyz="0.0 0.0 0.025"/>
</joint>

<link name="shoulder_2">
<visual>
<geometry>
<box size="0.05 .065 .105"/>
</geometry>
<material name="silver">
<color rgba="232 232 232 1"/>
</material>
</visual>
</link>

<joint name="shoulder_1_to_shoulder_1" type="fixed">
<parent link="shoulder_1"/>
<child link="shoulder_2"/>
<origin rpy="0 0.785398 0" xyz="0.022 0.0 0.03915"/>
</joint>

<link name="arm_joint_stevo1">
<visual>
<geometry>
<cylinder length=".09" radius="0.025"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>

<joint name="shoulder_2_to_arm_joint_stevo1" type="revolute">
<axis xyz="0 0 1"/>
<parent link="shoulder_2"/>
<child link="arm_joint_stevo1"/>
<origin rpy="1.5707963 0 0" xyz="0.00 0.00 0.025"/>
<limit effort="100" velocity="100" lower="-0.1899" upper="1.5707963"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="big_arm">
<visual>
<geometry>
<box size="0.05 .05 .13"/>
</geometry>
<material name="blue">
<color rgba="0 0 255 1"/>
</material>
</visual>
</link>

<joint name="arm_joint_stevo1_to_big_arm" type="fixed">
<parent link="arm_joint_stevo1"/>
<child link="big_arm"/>
<origin rpy="1.5707963 0 0" xyz="0.0 0.065 0.0"/>
</joint>

<link name="arm_joint_stevo2">
<visual>
<geometry>
<cylinder length=".06" radius="0.025"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>

<joint name="big_arm_round_to_joint_stevo2" type="revolute">
<axis xyz="0 0 1"/>
<parent link="big_arm"/>
<child link="arm_joint_stevo2"/>
<origin rpy="1.5707963 0 0" xyz="0.00 0.00 -0.065"/>
<limit effort="100" velocity="100" lower="1.0" upper="2.5891"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="arm_joint_stevo3">
<visual>
<geometry>
<cylinder length=".12" radius="0.012"></cylinder>
</geometry>
<material name="blue">
<color rgba="0 0 255 1"/>
</material>
</visual>
</link>

<joint name="arm_joint_stevo2_to_arm_joint_stevo3" type="revolute">
<axis xyz="0 0 1"/>
<parent link="arm_joint_stevo2"/>
<child link="arm_joint_stevo3"/>
<origin rpy="1.5707963 -0.52359877 0" xyz="0.00 -0.06 0.00"/>
<limit effort="100" velocity="100" lower="-1.5707963" upper="1.5707963"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="wrist">
<visual>
<geometry>
<box size="0.045 .04 .06"/>
</geometry>
<material name="blue">
<color rgba="0 0 255 1"/>
</material>
</visual>
</link>

<joint name="arm_joint_stevo3_to_wrist" type="fixed">
<parent link="arm_joint_stevo3"/>
<child link="wrist"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.09"/>
</joint>

<link name="arm_joint_stevo4">
<visual>
<geometry>
<cylinder length=".05" radius="0.02"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>

<joint name="wrist_to_arm_joint_stevo4" type="revolute">
<axis xyz="0 0 1"/>
<parent link="wrist"/>
<child link="arm_joint_stevo4"/>
<origin rpy="0 1.5707963 0" xyz="0.00 0.00 0.03"/>
<limit effort="100" velocity="100" lower="-1.5707963" upper="1.5707963"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="arm_joint_stevo5">
<visual>
<geometry>
<cylinder length=".04" radius="0.02"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>
<joint name="arm_joint_stevo4_to_arm_joint_stevo5" type="revolute">
<axis xyz="0 0 1"/>
<parent link="arm_joint_stevo4"/>
<child link="arm_joint_stevo5"/>
<origin rpy="0 1.5707963 0" xyz="-0.02 0.00 0.00"/>
<limit effort="100" velocity="100" lower="-1.5707963" upper="1.5707963"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="hand">
<visual>
<geometry>
<box size="0.04 .06 .02"/>
</geometry>
<material name="blue">
<color rgba="0 0 255 1"/>
</material>
</visual>
</link>

<joint name="arm_joint_stevo5_to_hand" type="fixed">
<parent link="arm_joint_stevo5"/>
<child link="hand"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.03"/>
</joint>

<link name="grip_joint_stevo6">
<visual>
<geometry>
<cylinder length=".05" radius="0.005"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>
<joint name="hand_to_grip_joint_stevo6" type="revolute">
<axis xyz="0 0 1"/>
<parent link="hand"/>
<child link="grip_joint_stevo6"/>
<origin rpy="0 1.5707963 0" xyz="0.00 0.015 0.00"/>
<limit effort="100" velocity="100" lower="-0.1762" upper="0.8285"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="grip_joint_stevo7">
<visual>
<geometry>
<cylinder length=".05" radius="0.005"></cylinder>
</geometry>
<material name="black">
<color rgba="255 255 255 1"/>
</material>
</visual>
</link>
<joint name="hand_to_grip_joint_stevo7" type="revolute">
<axis xyz="0 0 1"/>
<parent link="hand"/>
<child link="grip_joint_stevo7"/>
<origin rpy="0 1.5707963 0" xyz="0.00 -0.015 0.00"/>
<limit effort="100" velocity="100" lower="-0.8285" upper="0.1584"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

<link name="finger_left_big">
<visual>
<geometry>
<box size="0.04 .009 .045"/>
</geometry>
<material name="red">
<color rgba="194 194 194 1"/>
</material>
</visual>
</link>

<joint name="grip_joint_stevo6_to_finger_left_big" type="fixed">
<parent link="grip_joint_stevo6"/>
<child link="finger_left_big"/>
<origin rpy="0 0 0.34906585" xyz="0.03 0.0 0.0"/>
</joint>

<link name="finger_left_small">
<visual>
<geometry>
<box size="0.04 .009 .045"/>
</geometry>
<material name="red">
<color rgba="194 194 194 1"/>
</material>
</visual>
</link>

<joint name="finger_left_big_to_finger_left_small" type="fixed">
<parent link="finger_left_big"/>
<child link="finger_left_small"/>
<origin rpy="0 0 -0.34906585" xyz="0.04 -0.008 0.0"/>
</joint>

<link name="finger_right_big">
<visual>
<geometry>
<box size="0.04 .009 .045"/>
</geometry>
<material name="red">
<color rgba="194 194 194 1"/>
</material>
</visual>
</link>

<joint name="grip_joint_stevo7_to_finger_right_big" type="fixed">
<parent link="grip_joint_stevo7"/>
<child link="finger_right_big"/>
<origin rpy="0 0 -0.34906585" xyz="0.03 0.0 0.0"/>
</joint>
<link name="finger_right_small">
<visual>
<geometry>
<box size="0.04 .009 .045"/>
</geometry>
<material name="red">
<color rgba="194 194 194 1"/>
</material>
</visual>
</link>

<joint name="finger_right_big_to_finger_right_small" type="fixed">
<parent link="finger_right_big"/>
<child link="finger_right_small"/>
<origin rpy="0 0 0.34906585" xyz="0.04 0.008 0.0"/>
</joint>
</robot>

Leave a Reply

Scroll to top
%d bloggers like this: