上海交通大学：《ROS机器人操作系统基础与实战》课程教学资源（实验指导书）07 机器人建模与仿真

实验编号： 实验指导书 实验项目： 机器人建模与仿真 所属课程： ROS机器人操作系统基础与实战 课程代码： 面向专业： 机电专业 课程负责人： 朱笑笑 年月 日

实验编号： 实 验 指 导 书 实验项目： 机器人建模与仿真 所属课程: ROS 机器人操作系统基础与实战 课程代码: 面向专业: 机电专业 课程负责人: 朱笑笑 年 月 日

、 实验目的 让学生掌握创建虚拟机器人的方法，学会利用虚拟机器人简化我 们的机器人开发过程。 二、实验内容 1. 创建我们的机器人的3D模型 2. 为我们的机器人提供动作，物理限制，惯性和其他物理方面 0 添加虚拟的传感器到我们的3D模型 4. 在仿真器上使用模型 三、 实验过程或其他示意图 本次实验将创建一个具有四个轮子的移动机器人，同时还安装了一个带抓手的机械臂。 1. 创建一个URDF文件 首先，在urdf文件夹下创建robot1.urdf文件，输入以下代码。这种URDF文件是基于XML 格式的，可以使用专门的编辑器来进行编辑，我们这里直接使用gedit来进行编辑 -2-

- 2 - 一、 实验目的 让学生掌握创建虚拟机器人的方法，学会利用虚拟机器人简化我 们的机器人开发过程。 二、 实验内容 1. 创建我们的机器人的3D模型 2. 为我们的机器人提供动作，物理限制，惯性和其他物理方面 3. 添加虚拟的传感器到我们的3D模型 4. 在仿真器上使用模型 三、 实验过程或其他示意图 本次实验将创建一个具有四个轮子的移动机器人，同时还安装了一个带抓手的机械臂。 1. 创建一个URDF文件 首先，在urdf文件夹下创建robot1.urdf文件，输入以下代码。这种URDF文件是基于XML 格式的，可以使用专门的编辑器来进行编辑，我们这里直接使用gedit来进行编辑

-3-

- 3 -

在这个代码中，可以看到有两个主要的部分描述了这个移动机器人：Iink和joint 第一个link是名为base_link。该名字必须是唯一的。 为了定位在仿真其中将会显示的机器人内容，使用visual字段。在visual中可以通过定 义geometry(圆柱，立方体，球，等简单的形状，也可以是网格等自定义的或者由3维绘图 软件的生成的文件)，material(颜色或者纹理)，和原点。 Joint字段定义了两个link间的连接方式。可以定义joint的类型（固定，旋转，平移， 浮动，平面)，父连杆，子连杆和原点。在本实验中wheel_.1是base_Iink的一个子连杆。我 们将其设置为固定的，但是作为一个轮子也可以将其设置为旋转类型。 利用check_urdf检查urdf文件是否为问题。 S check urdf robot1.urdf robot name is:Robotl ---Successfully Parsed XML- root Link:base link has 4 child(ren) child(1):wheel 1 child(2):wheel_2 child(3):wheel 3 child(4):wheel 4 另外，通过图像化工具来查看urdf S urdf_to_graphiz"rospack find chapter5_tutorials'/ urdf/robot1.urdf 将会看到以下图像化输出 -4-

- 4 - 在这个代码中，可以看到有两个主要的部分描述了这个移动机器人：link和joint 第一个link是名为base_link。该名字必须是唯一的。 为了定位在仿真其中将会显示的机器人内容，使用visual字段。在visual中可以通过定 义geometry（圆柱，立方体，球，等简单的形状，也可以是网格等自定义的或者由3维绘图 软件的生成的文件），material（颜色或者纹理），和原点。 Joint字段定义了两个link间的连接方式。可以定义joint的类型（固定，旋转，平移， 浮动，平面），父连杆，子连杆和原点。在本实验中wheel_1是base_link的一个子连杆。我 们将其设置为固定的，但是作为一个轮子也可以将其设置为旋转类型。 利用check_urdf检查urdf文件是否为问题。 $ check_urdf robot1.urdf robot name is: Robot1 ---------- Successfully Parsed XML --------------- root Link: base_link has 4 child(ren) child(1): wheel_1 child(2): wheel_2 child(3): wheel_3 child(4): wheel_4 另外，通过图像化工具来查看urdf $ urdf_to_graphiz "`rospack find chapter5_tutorials`/ urdf/robot1.urdf" 将会看到以下图像化输出

base_link xyz:000 xyz:000 yz:000 xyz:000 py0-00 rpy:0-0 0 py0-00 pr0-00 base to wheell base to wheel2 base to wheel3 base to wheel4 wheel_1 wheel_2 wheel_3 wheel_4 2. 在rviz中观察3D模型 首先创建一个display..launch文件在1 aunch/文件夹下，输入以下代码 可以用以下命令调用riviz S roslaunch chapter7 display.launch model:="rospack find Chapter7/urdf/robot1.urdf" 正常情况下会出现以下画面 35 surhtee 制4tT3652r9rpt7B1 声06nm976价0的r07准61 尝试完成机械臂的urdf文件的设计，也可以查看己有的例程robotl.urdf。在rviz中可以看到以 下的效果。 -5-

- 5 - 2. 在rviz中观察3D模型 首先创建一个display.launch文件在launch/文件夹下，输入以下代码 可以用以下命令调用riviz $ roslaunch chapter7 display.launch model:="`rospack find Chapter7`/urdf/robot1.urdf" 正常情况下会出现以下画面 尝试完成机械臂的urdf文件的设计，也可以查看已有的例程robot1.urdf。在rviz中可以看到以 下的效果

下egB e3 incr3x关r4D2vl0o它c比drt: 32 oal Hropcrles TMotolgotlm D Ny told ■23,0119 60l Tak Tw城r…Ttoed Ttomme> 卡ul以.t-8K ,0仿，Cr01面量 Vens 令hGGK Cb以 川山El‘2 NormalLe...v Cellsie Con 厂22用22瓶22难 Npha 05 日”A Y S Dunent LiMf Drlete inin mcters omedde。 sduito me 江x a1Te13062337392AH716205nn=362539Tm320s4ret1712 3. 为urdf载入网格模型。 在geometry字段中我们简单的几何类型可以表示机器人的1ink,这样得到的结果并不美 观，我们可以在geometry中使用mesh格式的3D模型，来使得机器人模型更加逼真。这里 我们使用了一个P2机器人的抓手。在代码中可以看到如何实现的。 需要安装pr2 description包 Sudo apt-get install ros-kinetic-pr2_description 可以看到如下的效果， -6-

- 6 - 3. 为urdf载入网格模型。 在geometry字段中我们简单的几何类型可以表示机器人的link，这样得到的结果并不美 观，我们可以在geometry中使用mesh格式的3D模型，来使得机器人模型更加逼真。这里 我们使用了一个PR2机器人的抓手。在代码中可以看到如何实现的。 需要安装pr2_description包 Sudo apt-get install ros-kinetic-pr2_description 可以看到如下的效果

rie pors Hekt Nose Cemert ilereel Tlrtl ID kev csel 2 Ftos thlrmabe Daden 闪阿 Tucl Prtgertes utel Cpl ers va0 taw firel 世a山weu.■2sd.t1y Tusk rNto欢 :d5.alus OK ut.und rcde 年分he门X tp.. cel..10 Noms.Ce 0 Collsbm 5 Liruy Pione w1Tr13740n94423ww1F05t120 05Tm*1374n94423S19e14210 设置机器人模型为可运动模式 通过选择合适的joit的类型可以控制机器人模型的运动属性。在这个例子中使用到了多种 joint的类型。使用最多的joint是旋转joint。比如关节arm_lto_arm_base,代码如下 这块设置将该joint?定义为选择关节，可以实现连续的旋转，但是是有严格限制的。,表示上限1弧度，下限-1弧度，速 度0.5弧度/秒。旋转轴和与原点可以通过 及来设定。 子运行rviz的时候打开Join State Publisherf的GUI可以方便的观察urdf的可运动性能 S roslaunch chapter7 display.launch model:="rospack find Chapter7'/urdf/robot1.urdf"gui:=true 可以看到除了udf窗口，还有一个有多个滑动条的窗口，可以用来控制每一个关节。 5. 物理和碰撞属性 为了在Gazebo或任何其他仿真软件上模拟机器人，则需要添加物理和碰撞属性。这意味着 我们需要设置几何尺寸来计算可能的碰撞，例如，会给我们惯性的重量，等等。 -7-

- 7 - 4. 设置机器人模型为可运动模式 通过选择合适的joint的类型可以控制机器人模型的运动属性。在这个例子中使用到了多种 joint的类型。使用最多的joint是旋转joint。比如关节arm_1_to_arm_base，代码如下 这块设置将该joint定义为选择关节，可以实现连续的旋转，但是是有严格限制的。 ，表示上限1弧度，下限-1弧度，速 度0.5弧度/秒。旋转轴和与原点可以通过 及来设定。 子运行rviz的时候打开Join_State_Publisher的 GUI可以方便的观察urdf的可运动性能 $ roslaunch chapter7 display.launch model:="`rospack find Chapter7`/urdf/robot1.urdf" gui:=true 可以看到除了urdf窗口，还有一个有多个滑动条的窗口，可以用来控制每一个关节。 5. 物理和碰撞属性 为了在Gazebo或任何其他仿真软件上模拟机器人，则需要添加物理和碰撞属性。 这意味着 我们需要设置几何尺寸来计算可能的碰撞，例如，会给我们惯性的重量，等等

每一个ik都必须具有这些属性，否则无法对机器人模型进行模拟。 对于使用mesh网格的模型，最好是使比较简单的几何模型来实现碰撞检测，比实际的网格。 计算两个网格之间的碰撞比计算简单的几何更复杂。 以第一个轮子为例，添加了新的参数。其它关节也是类似的。完整的udf文件在 robotl physics.urdf … 6. 使用xacro简化模型文件 观察上一步完成的robotl physics.urdf的文件大小，我们总共使用了314行代码去定义我们的 机器人。假设我们后续要添加相机，腿和其它的几何结构，这个文件将会继续增大而使得文 件维护变的比较复杂。 Xacro能够帮助减少URDF文件的大小使其更加简洁易读和容易维护。同时也会方便我创 建一些相对比较重复的结构比如手臂、腿等。 在文件的开头什么一个xacro的属性，就可以xacro的功能。如下两行。这里还定义了对应 模型的名字robotl xacro。 请对照URDF的文件头，观察两者的区别，另外注意本文件的后缀为.xacro而不是.urdf。 使用常数 我们可以使用xacro声明常量值；因此我们可以避免在许多行中放入相同的值。没有使用 xacro,如果我们不得在每一个使用这个值的地方做修改，这样往往让维护变得非常的麻烦。 例如，四个轮子的长度和半径值相同。如果我们要改变这个值，我们需要在每一行中改变 它；但是如果我们使用下一行，我们可以很容易地改变所有的值： -8-

- 8 - 每一个link都必须具有这些属性，否则无法对机器人模型进行模拟。 对于使用mesh网格的模型，最好是使比较简单的几何模型来实现碰撞检测，比实际的网格。 计算两个网格之间的碰撞比计算简单的几何更复杂。 以第一个轮子为例，添加了新的参数。其它关节也是类似的。完整的urdf文件在 robot1_physics.urdf ... 6. 使用xacro简化模型文件 观察上一步完成的robot1_physics.urdf的文件大小，我们总共使用了314行代码去定义我们的 机器人。假设我们后续要添加相机，腿和其它的几何结构，这个文件将会继续增大而使得文 件维护变的比较复杂。 Xacro能够帮助减少URDF文件的大小使其更加简洁易读和容易维护。同时也会方便我创 建一些相对比较重复的结构比如手臂、腿等。 在文件的开头什么一个xacro的属性，就可以xacro的功能。如下两行。这里还定义了对应 模型的名字robot1_xacro。 请对照URDF的文件头，观察两者的区别，另外注意本文件的后缀为.xacro而不是.urdf。 使用常数 我们可以使用xacro声明常量值; 因此我们可以避免在许多行中放入相同的值。 没有使用 xacro，如果我们不得在每一个使用这个值的地方做修改，这样往往让维护变得非常的麻烦。 例如，四个轮子的长度和半径值相同。 如果我们要改变这个值，我们需要在每一行中改变 它; 但是如果我们使用下一行，我们可以很容易地改变所有的值：

而现在，要使用这些变量，你只需要改变旧的值 使用数学函数 可以使用四个基本操作(+，·，*，)，一元减号和括号在$}构造中构建任意复杂的表 达式。然而，指数和模数不受支持： 如果我们比较robotl.urdf robotl.xacro,我们将会消除30个重复的行，没有努力。可以 使用更多的宏和变量进一步减少它。 要使用xacro rviz和Gazebo,您需要将其转换为.urdf。为此，我们在chapter5 tutorials/urdf 文件夹中执行以下命令： S rosrun xacro xacro.py robotl.xacro robot1_processed.urdf 您也可以在任何地方执行以下命令，并且应该给出与其他命令相同的结果： S rosrun xacro xacro.py "rospack find chapter5_tutorials'/urdf/robot1. xacro">"rospack find chapter5 tutorials'/urdf/robot1_processed.urdf" 因此，为了使命令更易于编写，我们建议您继续使用同一个文件夹。 7. 利用代码来控制机器人 好的，我们有机器人的3D模型，我们可以在rviz上看到它，但是怎么能在一个node中控制机 器人呢？ 创建一个新的文件state publisher..cpp输入以下代码 #include #include #include #include int main(int argc,char**argv){ ros::init(argc,argv,"state publisher"); ros::NodeHandle n: -9

- 9 - 而现在，要使用这些变量，你只需要改变旧的值 使用数学函数 可以使用四个基本操作（+， - ，*，/），一元减号和括号在$ {}构造中构建任意复杂的表 达式。 然而，指数和模数不受支持： 通过使用数学，我们可以通过只改变一个值来调整模型的大小。 要做到这一点，我们需要 一个参数化的设计。 使用宏 宏是xacro最有用的组件，甚至更多，我们将使用下面的宏来表示惯性： 如果我们比较robot1.urdf robot1.xacro，我们将会消除30个重复的行，没有努力。 可以 使用更多的宏和变量进一步减少它。 要使用xacro rviz和Gazebo，您需要将其转换为.urdf。 为此，我们在chapter5_tutorials / urdf 文件夹中执行以下命令： $ rosrun xacro xacro.py robot1.xacro > robot1_processed.urdf 您也可以在任何地方执行以下命令，并且应该给出与其他命令相同的结果： $ rosrun xacro xacro.py "`rospack find chapter5_tutorials`/urdf/robot1. xacro" > "`rospack find chapter5_tutorials`/urdf/robot1_processed.urdf" 因此，为了使命令更易于编写，我们建议您继续使用同一个文件夹。 7. 利用代码来控制机器人 好的，我们有机器人的3D模型，我们可以在rviz上看到它，但是怎么能在一个node中控制机 器人呢？ 创建一个新的文件state_publisher.cpp 输入以下代码 #include #include #include #include int main(int argc, char** argv) { ros::init(argc, argv, "state_publisher"); ros::NodeHandle n;

ros::Publisher joint_pub=n.advertise("joint states",1); tf::TransformBroadcaster broadcaster; ros::Rate loop_rate(30); const double degree =M_PI/180; ∥robot state double inc=0.005,base_arm_inc=0.005,arml_armbase_inc=0.005, arm2_arml_inc=0.005,gripper_inc=0.005,tip_inc=0.005; double angle=0,base arm=0,arml armbase =0,arm2 arm1=0, gripper=0,tip=0; /message declarations geometry_msgs::TransformStamped odom trans; sensor_msgs::JointState joint_state; odom_trans.header.frame id ="odom"; odom trans.child frame id ="base link"; while (ros::ok()){ //update joint state joint_state.header.stamp ros::Time:now(); joint state.name.resize(7); joint_state.position.resize(7); joint state.name[0]="base to arm base"; joint state.position[0]=base_arm; joint state.name[1]="arm 1 to arm base"; joint state.position[1]=arml_armbase; joint state.name[2]="arm 2 to arm 1"; joint_state.position[2]=arm2_arml; joint state.name[3]="left gripper_joint"; joint_state.position[3]=gripper; joint state.name[4]="left tip joint"; joint_state.position[4]=tip; joint_state.name[5]="right_gripper_joint"; joint_state.position[5]=gripper; joint state.name[6]="right tip joint"; joint state.position[6]=tip; ∥update transform //(moving in a circle with radius 1) odom trans.header.stamp ros:Time:now(); odom trans.transform.translation.x cos(angle): odom_trans.transform.translation.y=sin(angle); odom_trans.transform.translation.z=0.0; odom_trans.transform.rotation=tf::createQuaternionMsgFrom Yaw( angle); //send the joint state and transform joint_pub.publish(joint state); broadcaster.sendTransform(odom_trans); //Create new robot state arm2 arml +arm2 arml inc; -10-

- 10 - ros::Publisher joint_pub = n.advertise("joint_states", 1); tf::TransformBroadcaster broadcaster; ros::Rate loop_rate(30); const double degree = M_PI/180; // robot state double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005, gripper_inc= 0.005, tip_inc= 0.005; double angle= 0 ,base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0, gripper = 0, tip = 0; // message declarations geometry_msgs::TransformStamped odom_trans; sensor_msgs::JointState joint_state; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; while (ros::ok()) { //update joint_state joint_state.header.stamp = ros::Time::now(); joint_state.name.resize(7); joint_state.position.resize(7); joint_state.name[0] ="base_to_arm_base"; joint_state.position[0] = base_arm; joint_state.name[1] ="arm_1_to_arm_base"; joint_state.position[1] = arm1_armbase; joint_state.name[2] ="arm_2_to_arm_1"; joint_state.position[2] = arm2_arm1; joint_state.name[3] ="left_gripper_joint"; joint_state.position[3] = gripper; joint_state.name[4] ="left_tip_joint"; joint_state.position[4] = tip; joint_state.name[5] ="right_gripper_joint"; joint_state.position[5] = gripper; joint_state.name[6] ="right_tip_joint"; joint_state.position[6] = tip; // update transform // (moving in a circle with radius 1) odom_trans.header.stamp = ros::Time::now(); odom_trans.transform.translation.x = cos(angle); odom_trans.transform.translation.y = sin(angle); odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw( angle); //send the joint state and transform joint_pub.publish(joint_state); broadcaster.sendTransform(odom_trans); // Create new robot state arm2_arm1 += arm2_arm1_inc;