R2机器人加载棋盘与棋子模型，对urdf、sdf的解释(区分srdf)

1、概述

urdf、sdf、srdf文件都属于xml的规范格式，解释分别如下：
urdf(unified robot description format)叫做"统一机器人描述格式"，主要目的就是提供一种尽可能通用的机器人描述规范，这样对于机器人的描述就可以互相移植，比较方便。

sdf(simulation description format)能够描述机器人、静态和动态物体、照明、地形甚至物理学的各方面的信息。sdf可以精确描述机器人的各类性质，除了传统的运动学特性之外，还可以为机器人定义传感器、表面属性、纹理、关节摩擦等，还提供了定义各种环境的方法，包括环境光照、地形，OpenStreetMaps中的街道以及The Prop Shop中提供的任何模型
从仿真角度讲，urdf文档不能描述不属于机器人的属性，即使其可能与机器人仿真十分相关，如环境光线、机器人在世界坐标系下的位姿、多个机器人之间的相对位姿等，算是对urdf的补充升级。

srdf(semantic robot description format)叫做"语义机器人描述格式"，是MoveIt针对控制机器人关节运动使用的一种机器人描述文件格式。有兴趣的可以查阅:ROS仿真R2机器人之安装运行及MoveIt的介绍 
使用命令：

rosrun moveit_setup_assistant moveit_setup_assistant

就可以打开MoveIt辅助安装工具，其中就有加载和生成srdf文件的操作。主要内容包括，关节组(joint groups)，默认状态配置(default robot configurations)，额外的碰撞检测信息(additional collision checking information)，额外的坐标系变换(additional transforms)等

我们重点来讲解下urdf对机器人的描述格式文件，连杆是带有质量属性的刚体，也就是不能发生形变，比如手臂，而关节是连接、限制两个刚体相对运动的结构，可以旋转，比如人的腕关节。关节也叫运动副。通过关节将连杆依次连接起来，就构成了一个个运动链，主要包括如下内容：

机器人模型的运动学与动力学描述
机器人的几何表示
机器人的碰撞模型 

2、urdf示例

 比如一个简单的描述如下：

<?xml version="1.0"?>
<robot name="mybot">  
    <link name="base_link">    
        <visual>      
            <geometry>        
                <cylinder length="0.8" radius="0.1"/>      
            </geometry>    
        </visual>  
    </link>
</robot>

那么这个机器人的名称叫mybot，link属性就是连杆的意思，所以是一根名为base_link的连杆，visual是可视化节点，geometry是几何学，对机器人关节的描述，这里就是该连杆的形状是cylinder圆柱体，其长度为0.8、横截面半径为0.1。
单位制度采用的是"米-千克-秒"，所以上面的长度和半径的大小是0.8米和0.1米。

上面的示例是一个连杆，再来看一个带关节的连接两个连杆的机器人：

<?xml version="1.0"?>
<robot name="mybot2">
    <link name="link_0">
        <visual>
            <geometry>
                <cylinder length="0.8" radius="0.1"/>
            </geometry>
        </visual>
    </link>
    
    <link name="link_1">
        <visual>
            <geometry>
                <cylinder length="0.8" radius="0.1"/>
            </geometry>
        </visual>
    </link>
    
    <joint name="joint_0" type="revolute">
        <axis xyz="0 0 1"/>
        <limit effort="100.0" lower="0.0" upper="0.5" velocity="0.5"/>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.3"/>
        <parent link="link_0"/>
        <child link="link_1"/>
    </joint>
</robot>

这里可以看到增加了一个名为joint_0的关节，类型是revolute，也就是说是可以旋转的。定义中我们知道是用来连接两个连杆的，可以叫做父连杆和子连杆，这里的父连杆是名为link_0，子连杆是名为link_1。
其中类型除了revolute之外还包括以下几种：

continuous：旋转，不受限制
revolute：旋转，转动角度受到限制
prismatic：平滑的
fixed: 固定关节
floating: 浮动关节，允许六个自由度的运动

3、棋盘与棋子

接下来我们看下sdf在实际当中的应用，我们在仿真平台中看下如何给下棋机器人画一个棋盘和棋子。下面一些代码是沿着上一篇文章的一些代码，是针对机器人的一些操作，有兴趣的可以一起看过来

3.1、bash操作

cd ~/chessbot/src
gedit r2_cli.py

#!/usr/bin/env python
import sys,rospy,tf,moveit_commander,random
from geometry_msgs.msg import Pose,Point,Quaternion
 
class R2Wrapper:
    def __init__(self):
        self.group={'left':moveit_commander.MoveGroupCommander('left_arm'),
                    'right':moveit_commander.MoveGroupCommander('right_arm')}
 
    def setPose(self,arm,x,y,z,phi,theta,psi):
        if arm!='left' and arm!='right':
            raise ValueError("unknow arm:'%s'"%arm)
        orient=Quaternion(*tf.transformations.quaternion_from_euler(phi,theta,psi))
        pose=Pose(Point(x,y,z),orient)
        self.group[arm].set_pose_target(pose)
        self.group[arm].go(True)
 
if __name__=='__main__':
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('r2_cli',anonymous=True)
    argv=rospy.myargv(argv=sys.argv)
    if len(argv)!=8:
        print('usage:r2_cli.py arm x y z phi theta psi')
        sys.exit(1)
    r2w=R2Wrapper()
    r2w.setPose(argv[1],*[float(num) for num in sys.argv[2:]])
    moveit_commander.roscpp_shutdown()

chmod u+x r2_cli.py
./r2_cli.py left 0.5 -0.5 1.3 3.14 -1.5 -1.57
./r2_cli.py right -0.4 -0.6 1.4 3.14 -1.5 -1.57
./r2_cli.py left 0.4 -0.4 1.2 3.14 -1.5 -1.57

这里是对机器人做一些移动操作，也可以将这些命令写入到bash文件中，r2.bash

#!/bin/bash
alias r2lhome="./r2_cli.py left 0.5 -0.5 1.3 3.14 -1.5 -1.57"
alias r2rhome="./r2_cli.py right -0.4 -0.6 1.4 3.14 -1.5 -1.57"
alias r2home="r2lhome;r2rhome"

然后通过source ./r2.bash 加载这些别名即可，这样就只需要输入 r2home，机器人就会规划一条通向起始位置的安全路径，并沿着它平滑地移动过去。对大多数机器人来说，一般都会设置一些常用的姿势，这样在日常操作、维护和任务当中都非常的实用方便。

启动一个空白的gazebo仿真界面

cd ~/chessbot
source devel/setup.bash
roslaunch gazebo_ros empty_world.launch

然后将R2机器人给加载进来

cd ~/chessbot
source devel/setup.bash
rosrun gazebo_ros spawn_model -file ~/chessbot/src/1.urdf -urdf -model r2

 接下来可以规划它运动了

cd ~/chessbot
source devel/setup.bash
roslaunch mybot move_group.launch
 
 
cd ~/chessbot
source devel/setup.bash
cd ~/chessbot/src
source ./r2.bash
r2home

3.2、画棋盘

gedit chess_board.sdf

<?xml version='1.0'?>
<sdf version='1.4'>
<model name='box'>
<!--<pose frame=''>1 0 2 0 0 0</pose>-->
  <static>true</static>
  <link name='link'>
    <collision name='collision'>
      <geometry>
        <box><size>0.5 0.5 0.02</size></box>
      </geometry>
      <surface>
        <friction>
          <ode>
            <mu>0.1</mu>
             <mu2>0.1</mu2>
          </ode>
        </friction>
        <contact>
          <ode>
            <max_vel>0.1</max_vel>
            <min_depth>0.001</min_depth>
          </ode>
        </contact>
    </surface>
    </collision>
   <visual name='visual'>
     <geometry><box><size>0.5 0.5 0.02</size></box></geometry>
   </visual>
  </link>
</model>
</sdf>

加载进来看下：

rosrun gazebo_ros spawn_model -file ~/chessbot/src/chess_board.sdf -sdf -model box

这里就是在sdf节点下，最常见的就是model节点，可以进行嵌套，这样就可以插入多个机器人了。可以看到节点属性比urdf多了，这也是其中一部分的节点，其余的还有light灯光效果，gravity重力加速度，scene场景参数，比如背景、阴影等，kinematic运动学等等。
其中static表示这个东西是静态的不能移动，pose可以自定义位姿，geometry里面定义物体的大小(长宽高)，collision碰撞检测里面包括了常见的物理特性，friction摩擦力，里面定义两个摩擦系数，contact节点里面的max_vel表示接触的最大速度，min_depth表示接触的最小深度 

3.3、画棋子

gedit chess_piece.sdf

<?xml version='1.0'?>
<sdf version='1.4'>
<model name='piece'>
  <link name='link'>
    <inertial>
      <mass>0.001</mass>
      <inertial>
        <ixx>0.0000001667</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.00000001667</iyy>
        <izz>0.0000001667</izz>
      </inertial>
    </inertial>
    <collision name='collision'>
      <geometry>
        <box><size>0.02 0.02 0.04</size></box>
      </geometry>
      <surface>
        <friction>
          <ode>
            <mu>0.4</mu>
             <mu2>0.4</mu2>
          </ode>
        </friction>
        <contact>
          <ode>
            <max_vel>0.1</max_vel>
            <min_depth>0.0001</min_depth>
          </ode>
        </contact>
     </surface>
   </collision>
   <visual name='visual'>
     <geometry><box><size>0.02 0.02 0.04</size></box></geometry>
   </visual>
  </link>
</model>
</sdf>

这里定义了棋子的质量mass为0.001，也就是1克，有一个惯性节点inertial，定义了物体的惯性矩阵，ixx, iyy, izz表示三个方向上的惯性，而ixy, ixz, iyz表示扭矩作用下的惯性力矩。
棋子比较多，我们使用代码循环生成棋子，落在棋盘上，这里会使用到服务，所以我们可以先来查看下有哪些服务列表：rosservice list，挺多的，如下图：

其中我们需要用到/gazebo/delete_model和/gazebo/spawn_sdf_model 

gedit spawn_chessboard.py 

#!/usr/bin/env python
import sys,rospy,tf
from gazebo_msgs.srv import *
from geometry_msgs.msg import *
from copy import deepcopy
 
if __name__=='__main__':
    rospy.init_node("spawn_chessboard")
    rospy.wait_for_service("gazebo/delete_model")
    rospy.wait_for_service("gazebo/spawn_sdf_model")
    delete_model=rospy.ServiceProxy("gazebo/delete_model",DeleteModel)
    delete_model("chessboard")
    s=rospy.ServiceProxy("gazebo/spawn_sdf_model",SpawnModel)
    orient=Quaternion(*tf.transformations.quaternion_from_euler(0,0,0))
    board_pose=Pose(Point(0.25,1.39,0.90),orient)
    unit=0.05
    with open("chess_board.sdf",'r') as f:
        board_xml=f.read()
    with open("chess_piece.sdf",'r') as f:
        piece_xml=f.read()
    print(s("chessboard",board_xml,"",board_pose,"world"))
    for row in [0,1,6,7]:
        for col in xrange(0,8):
            piece_name="piece_%d_%d"%(row,col)
            delete_model(piece_name)
            pose=deepcopy(board_pose)
            pose.position.x=board_pose.position.x-3.5*unit+col*unit
            pose.position.y=board_pose.position.y-3.5*unit+row*unit
            pose.position.z+=0.02
            s(piece_name,piece_xml,"",pose,"world")

这样就相当于先定位一个棋盘，然后就在棋盘中的对应位置，一个一个的画出棋子了。

加个可执行权限：chmod u+x spawn_chessboard.py
然后执行这个Python文件：./spawn_chessboard.p

success: True
status_message: "SpawnModel: Successfully spawned entity"