机械臂仿真控制实例
目录
- 环境设置
- 项目工具介绍
- Gazebo
（1）Gazebo组件
（2）Gazebo界面 - 统一机器人描述格式（URDF）
- RViz
- Moveit!
1.环境设置
对于此项目，使用的是 robo-nd 虚拟机映像，或者在本地安装了 Ubuntu + ROS 。
关于安装虚拟机映像问题，可以参考之前的文章。
对于此设置的其余部分，活动的ROS工作区的名称设置为catkin_ws。
如果没有活动的ROS工作空间，则可以通过以下方法创建：
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
建立了工作区之后，将项目存储库克隆或下载到工作区的src目录中：
$ cd ~/catkin_ws/src
$ git clone https://github.com/udacity/RoboND-Kinematics-Project.git
使用全能的 $ rosdep install命令安装缺少的依赖项：
$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
接下来，更改脚本文件的权限以使其变为可执行文件：
$ cd ~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/scripts
$ sudo chmod u+x target_spawn.py
$ sudo chmod u+x IK_server.py
$ sudo chmod u+x safe_spawner.sh
现在一切就绪，现在可以构建项目了。
在工作空间中，可以运行catkin_make来构建项目：
$ cd ~/catkin_ws
$ catkin_make
由于拾取和放置模拟器会在单独的终端中旋转不同的节点，因此需要将以下内容添加到 .bashrc 文件中以进行自动采购：
$ source ~/catkin_ws/devel/setup.bash
由于此项目使用自定义3D模型，因此需要告知Gazebo（仿真软件）在哪里寻找它们。由于环境变量，只需在 .bashrc 中添加一行即可轻松实现
打开一个终端窗口，然后输入以下内容：
$ echo "export GAZEBO_MODEL_PATH=~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/models" >> ~/.bashrc
对于演示模式，请确保在 ~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/launch/ 位置的 inverse_kinematics.launch 文件中将 demo 标志设置为true。
此外，还可以控制架子上目标对象的生成位置。为此，在下面的 target_description.launch 文件中修改 spawn_location 参数。文件在~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/launch/中。
0-9是 spawn_location 的有效值，0是随机模式。
这样就已经成功构建了我们的项目。
2.项目工具介绍
（1)Gazebo
Gazebo是一种基于物理学的3D仿真器，广泛用于机器人世界。在之前的测试ROS节点的时候我们已经使用过了。
（2)RViz
RViz，用于传感器数据分析和机器人状态可视化的3D可视化器，强调把已有的数据可视化显示。
（3）Moveit !
Moveit !，基于ROS的运动计划，运动学和机器人控制软件框架。
3.Gazebo
Gazebo是用于机器人技术的基于物理的高保真3D仿真器。Gazebo提供了在复杂的室内和室外环境中准确模拟一个或多个机器人的功能，该环境充满了静态和动态对象，逼真的照明和可编程的交互。
Gazebo有助于机器人设计，快速成型和测试以及真实场景的仿真。虽然Gazebo与平台无关，并且支持Windows，Mac和Linux，但它通常与ROS一起在Linux系统上与机器人系统一起使用。
简而言之，它是每个机器人专家必备库中必不可少的工具。有关Gazebo历史的详细信息和全面的功能列表，请访问他们的网站。
（1）Gazebo组件
运行Gazebo仿真实例所涉及的两个主要组件是 gzserver 和gzclient 。
gzserver完成了Gazebo的大部分繁重任务。它负责解析与我们要模拟的场景以及其中的对象相关的描述文件，然后使用物理和传感器引擎模拟整个场景。
通过在终端中使用以下命令可以独立启动服务器：
$ gzserver
它没有任何GUI组件。在某些情况下，以所谓的“无头模式” 运行gzserver可能会派上用场，但是在以后的课程中，我们将进一步讨论。
另一方面，gzclient提供了非常重要的图形客户端，该客户端可连接到gzserver并渲染仿真场景以及有用的交互工具。尽管可以使用以下命令从技术上本身运行gzclient：
$ gzclient
它根本不执行任何操作（消耗计算资源除外），因为它没有可连接的gzserver。
通常的做法是先运行gzserver，然后运行gzclient，在渲染模拟场景之前，先花一些时间初始化模拟场景，其中的对象以及相关的参数。为了使我们的生活更轻松，有一个直观的命令可以按顺序启动两个组件：
$ gazebo
(2)Gazebo界面
输入以下内容即可从终端启动Gazebo
$ gazebo
需要注意的是，有时候Gazebo会陷入一个看似无限的加载循环，如果Gazebo在一分钟内没有完全加载完，可以考虑在Gazebo终端处于活动状态时按Ctrl+C关闭它，然后重新启动它。
Gazebo gui分为4个主要部分：
- 场景
- 侧板
- 工具栏
- 菜单
场景
场景是您将花费大部分时间的地方，无论是创建仿真还是运行仿真。虽然可以使用触摸板在场景中导航，但强烈建议使用鼠标。您可以通过按下LMB并拖动来平移场景。如果你按住SHIFT键，你现在可以旋转视图。您可以使用鼠标滚轮或按下和拖动RMB来放大和缩小。
侧板
左侧的侧面板由三个选项卡组成：“World”，“Insert”和“Layers”。
- World
此选项卡显示场景中当前的灯光和模型。通过单击单个模型，可以查看或编辑其基本参数，例如位置和方向。此外，您还可以通过“Physics”选项更改场景的物理场，例如重力和磁场。GUI选项提供对默认摄像机视角和姿势的访问。 - Insert
在这里可以找到要添加到模拟场景中的对象（模型）。左键单击以展开或折叠模型列表/目录。要将对象放置在场景中，只需在“插入”选项卡下单击感兴趣的对象即可；这会将对象绑定到鼠标光标，可以通过在该位置单击鼠标左键将该对象放置在场景中的任何位置。 - Layers
这是一项可选功能，因此在大多数情况下此选项卡将为空。要了解有关图层的更多信息，请单击此处。
顶部工具栏
在顶部有一个工具栏，它可以快速访问某些光标类型，几何形状和视图。
- Select mode
最常用的光标模式，可用于浏览场景。 - Translate mode
更改对象位置的一种方法是在侧面板上的世界选项卡中选择对象，然后通过属性更改其姿势。这既麻烦又不自然，转换模式光标允许更改场景中任何模型的位置。只需选择光标模式，然后使用适当的轴来拖动对象直到满意为止。 - Rotate mode
类似于翻译模式，此光标模式允许更改任何给定模型的方向。
Scale mode
比例模式允许更改比例，从而更改任何模型的整体大小。 - Undo/Redo
由于人类最擅长犯错误，因此撤消工具可以帮助我们隐藏错误。另一方面，如果您撤消了您不想要的操作，请重做工具以进行撤销。 - Simple shapes
可以将诸如立方体，球体或圆柱体之类的基本3D模型插入场景。 - Lights
向场景添加聚光灯，点光源或定向光等不同的光源。 - Copy/Paste
这些工具可以在场景中复制/粘贴模型。另一方面，只需按Ctlr + C复制并按Ctrl + V粘贴任何模型。 - Align
该工具可以沿着三个主轴之一将一个模型与另一个模型对齐。 - Change view
使用该工具可从不同角度（例如俯视图，侧视图，正视图，仰视图）查看场景。
底部工具栏
底部工具栏有一个简洁的播放/暂停按钮。这使您可以暂停模拟以方便地移动对象。该工具栏还显示有关模拟的数据，例如模拟时间及其与实际时间的关系。您可以在此处找到FPS计数器，以评估任何给定场景的系统性能。
4.统一机器人描述格式（URDF）
统一机器人描述格式或urdf是ROS中用于表示机器人模型的XML格式。我们可以使用urdf文件来定义机器人模型，其动力学特性，视觉元素，甚至是机器人的模型传感器。URDF只能描述具有通过链节或树状结构的关节连接的刚性链接的机器人。它无法描述具有灵活链接或并行链接的机器人。
<?xml version="1.0"?>
<robot name="two_link_robot"><!--Links--><link name="link_1"><visual><geometry><cylinder length="0.5" radius="0.2"/></geometry></visual></link><link name="link_2"><visual><geometry><box size="0.6 0.1 0.2"/></geometry></visual></link><!--Joints--><joint name="joint_1" type="continuous"><parent link="link_1"/><child link="link_2"/></joint>
</robot>
以上是使用URDF描述具有两个连接和一个关节的简单机器人。
由于我们使用urdf文件描述了几种机器人和环境属性，因此阅读起来往往很长且乏味。这就是为什么我们使用Xacro（XML宏）将单个urdf文件分成多个xacro文件的原因。虽然语法保持不变，但是我们现在可以将机器人描述分为较小的子系统。对于此项目，我们仅将urdf分为两个xacro文件：
kr210.urdf.xacro-该文件包含所有机器人特定的信息，例如链接，关节，执行器等。kr210.gazebo.xacro该文件包含Gazebo特定的信息，例如机器人材料，摩擦常数以及在Gazebo中控制机器人的插件
由于kr210.urdf.xacro包含机器人特定的信息（如链接长度和关节偏移），因此它是唯一需要导出DH参数并创建变换矩阵的文件。由于urdf（和Xacro）文件基本上是XML，因此它们使用标记来定义机器人的几何形状和属性。最重要且最常用的标签及其元素如下所述：
<robot> </robot>
这是顶层标签，其中包含与给定机器人相关的所有其他标签。
<link> </link>
机械手中的每个刚性链接都必须与此标签关联。
属性(Attributes)
name：需要唯一的链接名称属性。
元素（Elements）
<visual> </visual>
该元素指定对象的外观以用于可视化目的。
| 名称 | 描述 |
|---|---|
<origin> |
视觉元素的参考框架相对于链接的参考框架。 |
<geometry> |
视觉对象的形状。 |
<material> |
视觉元素的材料。 |
<collision> </collision> |
连接的碰撞属性。请注意，这可能与连接的视觉属性不同，例如，通常使用更简单的碰撞模型来减少计算时间。 |
| 名称 | 描述 |
|---|---|
<origin> |
碰撞元素的参考框架，相对于链接的参考框架。 |
<geometry> |
请参阅以上视觉元素中的几何描述。 |
<inertial> </inertial> |
连接的惯性属性在此标签中描述。 |
| 名称 | 描述 |
|---|---|
<origin> |
这是惯性参考系相对于连杆参考系的姿势。惯性参考系的原点必须位于重心。 |
<mass> |
链接的质量由该元素的value属性表示。 |
<inertia> |
在惯性框架中表示的3x3旋转惯性矩阵。由于旋转惯量矩阵是对称的，因此在此处仅使用属性ixx，ixy，ixz，iyy，iyz，izz指定该矩阵的6个对角线以上的元素。 |
<link>具有重要元素的代码示例片段：
<link name="link_1"><inertial><origin xyz="0 0 0.4" rpy="0 0 0"/><mass value="${mass1}"/><inertia ixx="30" ixy="0" ixz="0" iyy="50" iyz="0" izz="50"/></inertial><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://kuka_arm/meshes/kr210l150/visual/link_1.dae"/></geometry><material name=""><color rgba="0.75294 0.75294 0.75294 1"/></material></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://kuka_arm/meshes/kr210l150/collision/link_1.stl"/></geometry></collision></link>
该<link>标签有许多可选元素，可以用来定义其他属性，如颜色、材质、纹理等。有关这些标记的详细信息，请参考此链接。
<joint> </joint>
该标签通常定义机器人中两个链接之间的单个关节。可以使用此标签定义的关节类型包括：
| 名称 | 描述 |
|---|---|
| 固定 | 没有自由度的刚性接头。用于将链接焊接在一起。 |
| 旋转 | 绕轴旋转的范围有限的关节。 |
| 棱柱 | 沿轴滑动的限位关节 |
| 连续 | 与“ 旋转关节” 相似，但没有限制。它可以绕轴连续旋转。 |
| 平面 | 2D 棱镜关节，允许在垂直于轴的平面中运动。 |
| 漂浮 | 具有6个自由度的关节，通常用于四旋翼和无人机 |
属性(Attributes)
name: 唯一的联合名称
type: 关节类型
元素（Elements）
要定义关节，我们需要声明旋转/平移轴以及形成关节的两个连接之间的关系。
| 名称 | 描述 |
|---|---|
<origin> |
这是从父链接到子链接的转换。关节位于子链接的原点。 |
<parent> |
各个关节的父级链接的名称。 |
<child> |
各个关节的子链接的名称。 |
<axis> |
定义旋转关节的旋转轴，棱柱形关节的平移轴和平面关节的表面法线。固定关节和浮动关节不使用轴字段。 |
<joint>具有重要元素的代码示例片段：
<joint name="joint_2" type="revolute"><origin xyz="0.35 0 0.42" rpy="0 0 0"/><parent link="link_1"/><child link="link_2"/><axis xyz="0 1 0"/>
</joint>
<joint>标签下的其他可选元素可以在此处找到。
还有更多可选标签和属性，可帮助定义机器人以及传感器和执行器的各种动态和运动学特性。参考此链接以获取关于这些的更多详细信息。
5.RViz
RViz(或RViz)代表ROS可视化工具或ROS可视化器。RViz是一站式工具，可以可视化机器人的三个核心方面:感知、决策和驱动。
使用rviz，我们可以可视化发布在ROS主题上的任何类型的传感器数据，如相机图像、点云、超声波测量、激光雷达数据、惯性测量等。这些数据可以是直接来自传感器的实时流，也可以是作为bagfile存储的预记录数据。
通过RViz还可以可视化机器人的活关节角度值，从而构建任何机器人的实时三维表示。尽管如此，rviz并不是一个模拟器，也不与物理引擎接口，换句话说，没有碰撞和动力学。RViz并不是Gazebo的替代品，而是一个辅助工具，可以监视机器人系统下的运动每一个过程。
由于rviz是ROS软件包，因此需要运行roscore才能启动rviz。
$ roscore
在另一个终端中，运行rviz：
$ rosrun rviz
正确启动后，rviz窗口应如下所示：
中间的空白窗口称为3D视图(3D view)，在其中来观察机器人模型，传感器可视化和其他元数据。
左侧的面板是已加载的显示(Displays)的列表，而右侧的面板显示了可用的不同视图(Views)。
在顶部，我们提供了许多有用的工具(tools)，底部的栏显示了有用的信息，例如经过的时间，fps计数以及所选工具的一些方便的说明/细节。
(1) 显示（Displays）
要使任何内容都显示在3D视图中，首先需要加载正确的显示（Displays）。
显示（Displays）还可以用于可视化传感器数据流，例如3D点云，激光雷达扫描，深度图像等。
默认情况下，Rviz以两个无法删除的固定属性字段启动- 全局选项和全局状态。虽然不显示这些内容，但一个管理简单的全局设置，而另一个则检测并显示有用的状态通知。
让我们来看看一些基本的显示（Displays）。我们已经加载并启用了网格显示。要添加机器人模型显示，首先需要将机器人描述（urdf）加载到参数服务器中，并在所有机器人链接之间发布转换。幸运的是，我们有一个方便的启动文件，可以为我们完成所有这些工作，打开一个新终端，然后键入以下命令：
$ roslaunch kuka_arm load_urdf.launch
如果提示找不到文件，则添加一下环境变量，之后继续运行上面的命令就可以了：
source ~/catkin_ws/devel/setup.bash
现在，让我们回到rviz窗口并通过单击Add 底部的按钮添加显示。这将打开一个带有显示类型的新窗口，选择RobotModel并点击Ok。接下来，在Displays中将“ Fixed Frame”下的“ Global Options” 从world（或map）更改为base_link。
如果一切正常，应该可以看到机器人模型：
注：若是报错未找到文件，很多情况下都是由没有添加环境变量导致的
可以通过取消选中旁边的复选框来禁用显示类型，而不必完全删除它。通过选中复选框再次启用。
可以随意使用不同的显示类型及其属性，但请记住，要使大多数显示类型正常工作，需要加载相应的源。
(2) 视图（Views）
rviz中有几种视图类型，它们基本上可以更改3D视图中的相机类型。请记住，对于每种视图类型，都可以在底部状态栏看到有关如何使用鼠标旋转，平移和缩放的说明。我们将在这里讨论三种广泛使用的相机类型：
1)轨道（Orbit）
在轨道视图中，你设置一个焦点，相机就会围绕这个焦点旋转，一直看着它。当移动时，你可以看到一个黄色圆盘状的焦点。
2) 第一人称 FPS
FPS视图是第一人称摄像机视图。就像FPS视频游戏一样，摄像头会旋转，就像绕着你的头旋转一样。
3) 上下正交（TopDownOrtho）
该摄像机始终沿全局Z轴向下看，从而将摄像机的移动限制在XY平面上。通常在为移动机器人执行2D导航时使用。
（3）工具栏
接下来，我们将探索rviz窗口顶部工具栏中的一些工具。尽管这些工具大多数都是通用工具，可用于任何机器人，但其中一些工具专用于移动机器人导航，将在以后的课程中介绍。
1）移动相机
这是最基本的工具，通常是默认工具，它用于移动相机。请记住，运动控件从一种视图类型更改为另一种视图类型，请再次参考运动表以快速比较控件。
2）选择
为了演示该工具，让我们添加一个名为“选择面板”的新面板。为此，请在rviz窗口的顶部菜单上单击“面板”，然后单击“选择”，现在应该在显示面板顶部看到一个空的“选择”面板。现在，从工具栏中选择“选择”工具。使用此工具，可以通过左键单击来选择单个项目，也可以通过单击并拖动以选择多个项目。选择的详细信息显示在“选择”面板中。例如，选择一个机器人链接将提供其当前姿势（位置+方向）。在需要确定一个或多个项目的属性或状态的复杂环境中，此工具非常有用。
3）对焦相机
顾名思义，此工具将机器人的任何项目或部分置于“焦点”位置，这意味着只需单击一次即可将其置于3D视图面板的中心。
4）测量
激活后，此工具可测量3D视图中两点之间的距离。可以通过单击鼠标左键设置起点，然后通过单击鼠标左键设置终点。这两个点之间的测量距离将显示在rviz窗口的底部状态栏中。但是请记住，无法测量空白空间中两点之间的距离。所以必须具有一些对象才能单击。
5）相互作用
这是一个特殊的工具，可用于与交互式标记交互。在此处可以了解更多有关交互式标记的信息。
6.Moveit!
MoveIt！是用于操纵，运动学和控制的高级运动计划框架。它提供了一个平台，用于开发先进的机器人应用程序，评估新的机器人设计以及构建适用于工业，商业，研发和其他领域的集成机器人产品。
我们将利用Moveit的各个组件，因此，对于整个计划中的不同项目，我们将探讨Moveit的各个组成部分。
场景规划
规划场景表示机器人的状态以及周围世界的状态。
它通过订阅joint_states主题来跟踪机器人状态。它集成了来自各种传感器（如立体声摄像头和/或RGBD摄像头）的信息，以实质上创建机器人周围动态环境的3D地图。
也可以通过将抽象对象或虚拟对象发布到特定主题来将其插入到所谓的3D地图中。
运动规划
MoveIt！没有内置的运动计划算法。相反，它提供了一个方便的插件界面来与各种运动计划库进行通信和使用。
Moveit！利用特殊的ROS服务与任何给定的运动计划者建立请求-响应关系。
与pub-sub不同，请求-响应关系依赖于在两个节点之间传递的非常特定的消息。在这种情况下，一旦选择了运动计划器并通过插件界面加载了运动计划器，则可以向计划器发送请求消息，其中包含有关如何生成计划的特定说明。
响应我们的请求消息，Moveit！使用上面讨论的“规划场景”生成所需的无碰撞轨迹。
需要注意的一点是，运动计划器仅生成从机器人的开始状态到其结束状态的无碰撞路径，然后生成Moveit!。将此路径转换为遵循关节角度的速度和加速度约束的关节空间轨迹。
一旦有了有效的无碰撞轨迹，就可以使用适合您的机器人的控制器来执行它。
运动学
在运动计划过程的各个阶段，Moveit！需要使用正向运动学将机器人的关节状态转换为末端执行器姿势，反之亦然。
ROS的RobotState类提供对正向运动功能的访问，而Moveit！允许用户编写自己的逆运动学实现。这是之后将要实现的部分，首先是数学上的实现，然后是python脚本的形式。
我们要编写的python脚本只为特定的6DOF机械臂（Kuka KR210）解决反向运动学问题（IK），但是有通用库和插件可用于为不同类型的机器人解决IK。几个广泛使用的解决方案是：
- orocos开发的 运动学和动力学库（KDL）
- OpenRAVE开发 的 IKFast Slover
move_group节点
move_group节点是Moveit！的主节点！包装并用作中央枢纽，提供Moveit几乎所有组件之间的通信。
它以Rviz GUI的形式提供用户界面，并就各种主题与机器人进行对话，以获取诸如机器人状态，转换，传感器数据，控制器状态等信息。我们将在未来的运动计划模块中广泛讨论该节点。 。