文章目录
- 0、安装机械手臂
- 1 Robotics-Tutorials
- 一、How to Start Learning Robotics【General】
- 二、The Best Projects to learn Robotics【General】
- 三、ROS Tutorial: Visualize the UR5 robot in Rviz – URDF explained【Basics】
- 四、ROS Tutorial: Simulate the UR5 robot in Gazebo – URDF explained【Basics】
- 五、ROS Tutorial: Control the UR5 robot with ros_control – How to tune a PID Controller【Control】
- 六、Tutorial: Control the TCP position of a UR5 robot in C++ with KDL – Inverse Kinematics explained【Control】
- 七、ROS Tutorial: How to create a Moveit config for the UR5 and a gripper【Motion Planning】
- 八、ROS Tutorial: Pick and Place task with the Moveit C++ interface【Motion Planning】
- 九、ROS Tutorial: How to use a depth camera with Moveit for collision avoidance【Motion Planning】
- 十、ROS Tutorial: How to use OpenCV in a Robot Pick and Place task for Computer Vision【Computer Vision】
- 10.1 What is OpenCV and how to install it?
- 10.2 How to set up and run the Pick and Place task
- 10.3 Creating the OpenCV node
- 10.3.1. Converting the camera images between ROS and OpenCV with the cv_bridge package
- 10.3.2. Applying Computer Vision algorithms to the images
- 10.3.3. Extracting boundaries and their center from the image
- 10.3.4. Extracting the object positions
- 10.3.5. Converting the 2D pixel to a 3D position in the world frame
- 10.3.6. Sending the resulting positions as a service call
- 10.4 How to call the OpenCV node service from the Pick and Place node
0、安装机械手臂
官方教程：RoboticsTutorial
前人教程：UR5机械臂仿真实例
Ros安装，可以使用 鱼香一键安装
rosdep update 出错，使用小鱼的大佬的 一键配置
wget http://fishros.com/install -O fishros && . fishros
1、安装中的错误
A、安装依赖时出错 rosdep install
rosdep install --rosdistro melodic --ignore-src --from-paths src
#All required rosdeps installed successfully
ERROR: the following packages/stacks could not have their rosdep keys resolved to system dependencies: cvg_sim_gazebo_plugins: Cannot locate rosdep definition for [gazebo_plugin]
解决方案：
在安装依赖时出现此问题。
说明是rosdep找不到文件，注意名称书写是否有问题，版本是否对应的上。
实施这个 rosdep install --from-paths . --ignore-src -r -y
20240426 问题找到：是roscore无法执行成功。因为每一个新的中断都需要执行问题C的方案。【目前此问题已经解决，roscore正常使用中】。
B、编译是的错误：
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "move_base_msgs" with any of the following names:
move_base_msgsConfig.cmake
move_base_msgs-config.cmake
网上有很多类似的错误，但是要注意看names后面是什么，有时候就算代码（83）一样，缺的包也是会不一样的。
————————————————
原文链接：https://blog.youkuaiyun.com/CHNNNNNNNN/article/details/136262803
解决方案：
安装缺失的包即可：
sudo apt-get install ros-melodic-move-base-msgs
C、catkin_make not found
执行catkin_make 是报错
Command 'catkin_make' not found, but can be installed with:sudo apt install catkin
解决方案：[可能是某些操作删除了该工具]
source /opt/ros/melodic/setup.bash
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc
D、报错：vmw_ioctl_command error 无效的参数
运行：roscore & rosrun gazebo_ros gazebo 或者 terminal 直接输入：gazebo即可；
打开gazebo时出现报错：vmw_ioctl_command error 无效的参数
解决方案：
在~/.bashrc文件写入export SVGA_VGPU10=0，即：通过输入以下命令，关闭硬件加速功能
2、运行ur5机械手臂时候的报错。
运行 gazebo_ros_control.launch时，错误如下：
[ERROR] [1571212497.739111392, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/shoulder_pitch_joint
[ERROR] [1571212497.744784033, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/elbow_roll_joint
[ERROR] [1571212497.747626326, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/elbow_pitch_joint
[ERROR] [1571212497.749455772, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/wrist_roll_joint
[ERROR] [1571212497.751916855, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/wrist_pitch_joint
[ERROR] [1571212497.757109573, 0.166000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/gripper_roll_joint
可忽略此错误，原因见参考文献。
————————————————
原文链接：https://blog.youkuaiyun.com/u011304078/article/details/102588040
解释见：No p gain specified for pid
1 Robotics-Tutorials
官方教程：roboticscasual
一、How to Start Learning Robotics【General】
ROS教程一： 如何学习机器人？
你对机器人感兴趣吗？你想制造会动、会感觉、会思考的机器人吗？太好了！你不确定从哪里开始吗？那这篇文章就送给你了！
机器人是一个包含多个学科的多样化领域，因此没有一个单一的起点。在文章的开始，我将概述机器人学中最重要的主题。之后，我们将讨论开始学习过程的最佳方法(通过做自己的项目)，我将为您指出从哪里开始的具体资源。在做这些项目的时候，你会顺便学到一些理论。我们将讨论如何最有效地做到这一点，并向您展示一些最好的在线资源。最后，我们将讨论如何更专业地做机器人，并成为一名机器人开发者。让我们开始你的学习之旅吧！
The Different Disciplines in Robotics
一般来说，机器人技术的发展都与机械、电子和编程有关。它们中的每一个都涵盖了机器人开发中不同但同样重要的部分。力学是关于机器人的机械性能。它是移动的吗（例如，它有轮子吗）？是机器人手臂吗？它能举起多少重量？这一切都取决于机器人的机械设计。
电子学处理电动机的控制以使机器人移动。它还关注机器人的传感器，这些传感器用于测量物理世界中的事物，从而获得对环境的感知。
编程使机器人变得智能。它是以代码的形式告诉机器人如何移动以及如何使用传感器信息。它还旨在使机器人能够做出自己的决定。如果你想知道机器人领域最常用的编程语言是什么，请查看这篇文章。
为了从头开始建造一个真正的机器人，你需要处理每一个问题，但不要担心，你不需要一次学会所有东西。在下一段中，我们将讨论让学习机器人变得有趣和有动力的最佳方法。
The Best Learning Approach for Beginners is Learning by Doing
对初学者来说，最好的学习方法是边做边学
在开始的时候，不要打开一本理论教科书去学习算法。这种方法很快就会变得非常令人沮丧。相反，从项目开始。项目很有趣，最终你有东西可以展示！这样你就有了一个明确的目标，你的学习之路就会变成你实现目标所需要知道的一切。因此，在做酷项目时，你会获得知识作为副产品！
有各种各样的项目可以开始。你可以开始用Arduino微控制器板或Raspberry Pi单板计算机构建一个简单的机器人。当你从零开始用一个简单的底盘、一些电机和传感器以及一个简单的处理单元制造一个这样的机器人时，你可以同时学到机械、电子和编程知识。您可以购买完全组装的机器人或机器人套件来自己组装机器人。您还可以从头开始设计某些部件，比如自己的3D打印部件，或者创建自己的电路。这样你就可以调整难度水平，决定你想更详细地了解什么。
然而，如果你不想花钱或者你对机器人技术的机械/电子部分不太感兴趣，你就不需要一个物理机器人。事实上，你需要的只是一台电脑。今天，你可以在你的电脑上模拟机器人，这几乎和拥有一台实体电脑一样有趣。这不需要前期投资，而且有大量的在线资源可以指导您。现代机器人技术的一个幸运发展是机器人操作系统的发展。它是一个用于机器人的开源操作系统，在社区甚至专业机器人公司中广泛使用。在这个博客上，我们有一堆关于如何使用ROS进行机器人开发的教程，请在这里查看。
如果你想要一个更详尽的列表和对你可以开始的项目的详细描述，我有一篇关于开始学习机器人的最佳项目的文章，其中包括有硬件和没有硬件的项目。
But what about the theory?
但是你只有知道你正在做的事情背后的理论，你才能彻底理解这些事情，对吗？这是一项崇高的努力，但也可能非常令人畏惧和沮丧。如果你试图从一开始就理解一切，你会很快不知所措。当然，当你做这些项目时，问题就会出现。当这种情况发生时，对它们做一些研究。通常快速的网上搜索会在几分钟内给你一个答案。如果这些话题中有一些变得更加复杂，不要担心。对于项目来说，通常不需要理解每一个细节，所以如果你觉得没有把握住，你可以继续前进。
另一方面，如果某样东西激发了你的兴趣，那么你应该利用它，深入挖掘。有时，你会在基础的基础上结束，如运动学，动力学，控制或运动规划。
这些是通常在教科书中涉及的基础知识。如果你在寻找答案的过程中得到了这些基本原则，那么你不妨看看文献。然而，现在你会明白这个问题从何而来，以及它与一个真实的机器人项目有何关联。这将使学习理论更容易，更有动力。如果你不是一个爱读书的人，你就不需要拘泥于课本。有很多在线资源，从在线讲座、教程视频到博客文章，都非常擅长解释理论概念。要获得详尽的列表，请查看我关于学习机器人的最佳在线资源的文章。
The Path to become a professional robotics developer
现在让我们假设你已经处理过实际项目，甚至可能有一点理论。也许你已经找到了真正让你感兴趣甚至兴奋的话题。如果你已经在考虑从事机器人专业是否适合你，我想在这里给你一些指导。从事机器人专业工作很有趣，也是一份伟大的职业，成为机器人开发人员有很多途径。一般来说，拥有技术大学/学院学位会有所帮助。哪种学位在很大程度上取决于你想从事的机器人领域。我写了一篇关于从事机器人工作的最佳学位的文章，其中也涵盖了你将在这些学位中学到什么以及它们将教给你哪些有价值的机器人技能。
不过，要想在机器人领域找到工作，你不一定需要有一个完整的工程/计算机科学学位。例如，如果你知道如何编程，与ROS合作过，并对机器人基础知识略知一二，这已经是机器人职业生涯的良好基础。转行者总是有机会进入这个行业。
Conclusion
我希望这份指南对你开始机器人学习之旅有所帮助。按照我描述的学习过程是非常有益的，最终会给你最好的体验。即使你有时可能会遇到困难，你也总能在网上找到帮助，因为有一个很棒的机器人社区随时准备为你提供指导。机器人技术是非常有益的，它是为每个人服务的。你可以从最简单的项目开始学习，有一天你可能会在该领域获得博士学位。无论你正处于学习旅程的哪个阶段，本网站都会尽力为每个人提供有用的信息和指导。所以一定要看看我博客上的其他文章🙂
二、The Best Projects to learn Robotics【General】
ROS教程二： 学习机器人最好的项目
在之前的一篇文章中，我谈到了如何开始学习机器人技术。今天我们将探讨一些最有趣的项目创意。建造东西比阅读教科书有趣得多，你将在途中探索理论。从事机器人技术的门槛降低了很多，因为现在你只需要一台电脑，甚至不需要任何电子知识。有时，从哪里开始会令人困惑。在机器人领域，有成千上万个可能的项目和各种不同的主题可供您探索。在这篇文章中，我将概述你可以做的不同类别的项目，并介绍一些最有趣的项目。我们将介绍带有嵌入式主板的DIY机器人，如Raspberry Pi或Arduino，更先进的机器人以实验机器人操作系统（ROS）和您可以完全在模拟中完成的项目！
DIY Robot Projects with Rasperry Pi or Arduino树莓派/单片机
Arduino和Raspberry Pi是两种嵌入式计算机板，非常适合构建机器人。它们体积小，易于编程，尽管占地面积小，但具有相当大的计算能力。您可以轻松地将它们安装在不同的机器人底盘上，并连接电机和各种传感器。你的想象力是你的极限。
Arduino/单片机
Arduino板没有Raspberry Pi强大，但可能更容易设置，也更便宜。以下在circuitdisgest上展示的Arduino项目展示了如何建造一个能够避开障碍物的自动驾驶机器人:
它使用超声波传感器来测量到下一个物体的距离。如果对象要关闭，它会自动更改目录。一个arduino板只需要6美元，一个简单的机器人底盘，包括在他们的设置中使用的电机，只需要15美元，而一个超声波传感器是2美元。所以总而言之，这是一个便宜，简单，有趣的起步项目。
Raspberry Pi/树莓派
树莓派更像是一台功能强大的迷你电脑，可以运行像Linux这样的操作系统。在这个线跟踪项目中，他们将一个红外传感器连接到RasPi，使机器人能够跟踪地板上的黑线。这是一个非常常见的用例，例如仓库机器人。他们使用Python为机器人编程，这是一种编程语言，易于开始，但在专业机器人应用中使用很多。
还有很多其他很酷的DIY机器人项目，比如手势控制的机器人手臂或可以通过智能手机甚至语音控制的机器人。你看，这些简单的嵌入式主板的机会是无限的。
这些DIY项目是学习电子、传感器、致动器以及接近硬件的编程的好方法。如果你不想自己组装和布线，也有很多商店提供廉价的机器人套件，例如机器人商店。
Robot platforms running on ROS
ROS（机器人操作系统）是一个用于机器人应用的开源软件框架，近年来广受欢迎。它被广泛用于大学机器人教学，甚至被专业机器人公司使用（点击此处了解更多信息）。围绕ROS有一个很棒的社区，提供教程、帮助和现成的实现。然而，为了在机器人上运行ROS，一些带有操作系统的计算能力是必要的。你可以在你的计算机上进行纯粹的ROS模拟实验，我们将在下一节中讨论这一点，但也有专门用于ROS的真实机器人。
TurtleBot
可能最受欢迎的是TurtleBot，它的最新版本3是由Robotis开发的。这是一个移动平台，主要用于导航，但也可以连接一个手爪来完成操作任务。最小版本的起价为549美元，这一价格并不算低，但当你看到这些功能时，它会变得更有吸引力。在其他组件中，它配备了强大的电机，相当多的计算能力，电池组和360度激光雷达，允许建立一个环境地图(使用SLAM算法)。更昂贵的版本甚至有一个摄像头来试验计算机视觉应用。所有这些使得TurtleBot在研究和教育领域非常受欢迎，所以如果你正在寻找一个在真实机器人上轻松进入ROS的方法，你可能会仔细看看。
Robot Arm
如果你正在寻找一个机器人手臂，看看特罗森机器人公司能提供什么。他们最小的版本PincherX 100起价549美元。它的有效载荷非常小，只有50克，并不是用来制造汽车的，但也许通过一点编程，它可以帮助你用回形针整理你的桌子。因为它是专门为与ROS框架一起使用而开发的，所以有很多资源可以帮助您入门。同样值得一试的是他们产品范围的其他部分，他们提供不同价格类别的不同种类的机器人套件。
你可以在下面的链接中找到一个详尽的机器人列表
最后，如果你想自己制作机器人，可以在Raspberry Pi上安装ROS！由于缺乏计算能力，存在一些限制，但对于各种项目来说仍然足够了。一个例子是在驾驶机器人平台上连接Raspberry Pi，安装ROS并连接XBOX控制器以控制它。Wil Salby在他的博客上描述了这个项目，他还在Raspberry Pi上安装了一个摄像头，用于视觉应用。
Projects with simulated Robots
让我告诉你一些真正令人惊讶的事情:市场上的许多机器人都有模拟模型可用于gazebo和其他模拟器。它们中的许多都可以免费下载，就像Gazebo一样，它允许我们免费玩机器人应用程序！该模拟打开了一个全新的机会世界，因为它允许在现实世界的物理影响下，在不同的环境中在我们的计算机上运行机器人。我们甚至可以模拟传感器数据输入，例如来自3D摄像机的数据。这使得模拟适用于各种项目，包括视觉、感知、运动规划、控制、机器学习等！
在这个博客上，我为Universal Robots的UR5机器人项目写了教程。如果你是gazebo或机器人模拟的新手，首先浏览这些或ROS wiki上的内容肯定是有意义的。在那里，您可以学习如何创建自己的机器人模型以及如何使用move it移动它！包裹。当你准备好接受更雄心勃勃的项目时，也有很多很好的资源可以帮助你。让我向您展示一些在模拟中完成的不同高级主题的有趣机器人项目示例:
Kuka机器人的机器人运动学
Nick Hortovanyi在他的medium博客上写了关于Udacity的机器人纳米学位计划的项目。其中一门是关于机器人运动学的高级课程。他经历了推导DH参数和构建变换矩阵以计算正向运动学的过程。在此基础上，他编写了一个程序来计算反向运动学，以控制Kuka机器人的抓取位置并执行拾取和放置任务。
使用TurtleBot3进行机器学习
早些时候，我们谈到了机器人3。他们在其网站上提供指导项目，也用于在模拟中使用机器人。一个有趣的项目是使用机器学习来教机器人避开障碍物。特别是，他们使用强化学习，这是机器学习的一种特殊形式。他们展示了如何安装Tensorflow和Keras以及必要的软件包，如何训练模型以及如何可视化结果。机器学习本身是一个全新的世界，目前很可能是机器人领域最热门的话题，因此值得探索！
PR2机器人的3D感知
Udacity的机器人纳米学位项目的另一个项目是机器人感知。Nick Hortovanyi描述了识别桌子上物体的项目。使用的是模拟的PR2，这是在willow garage(ROS的创造者)开发的双臂机器人。PR2机器人有一个深度相机，可以提供点云来表示环境。然后处理该点云，并应用各种过滤器来提取所需的信息。最后，训练一个经典的机器学习模型来标记对象。
我刚刚描述的项目是高级的，面向更有经验的ROS用户。然而，目的是给你一些动力，并向你展示在模拟中工作的所有可能性。一旦你开始学习基本的ROS和Gazebo教程和小项目，你会自动获得知识并发现更多你感兴趣并想探索的主题。
在本文中，我试图向您概述最适合学习机器人技术的机器人项目。我试图涵盖具有不同先决条件和范围的不同项目类别。我希望它对你有所帮助，现在你有了一个可以开始搜索和选择最适合你的项目的起点。如果你知道其他值得一试的酷机器人项目，请把它们写在下面的评论中！
三、ROS Tutorial: Visualize the UR5 robot in Rviz – URDF explained【Basics】
ROS教程三： 在Rviz中可视化UR5机器人—解释URDF
好消息！你不需要一个真正的机器人来开始学习机器人技术。Rviz是一个ROS工具，用于在您的计算机上虚拟显示机器人。你所需要的只是你的机器人模型的虚拟描述，一个所谓的URDF。在本文中，我们将从Rviz的Universal Robots中看到UR5机器人，并看看它的URDF。我们还将讨论机器人运动学，这是一个基本概念，在学习机器人技术时理解起来很重要。听起来不错？开始吧！
注意:如果您只想在Rviz中启动并运行UR5，而没有阅读更多关于URDFs和机器人运动学的内容，您可以直接跳到快速入门部分，在那里您可以找到立即开始的必要命令。否则，让我们马上开始吧！
3.1 Installing an launching Rviz with the UR5 robot
使用UR5机器人安装发射Rviz
我假设您已经在Linux操作系统上安装了您选择的ROS发行版。如果没有，请按照这里的描述进行操作。如果尚未安装Rviz(通常与ROS安装一起安装)，只需运行:
格式如下：
sudo apt-get install ros-$ROS_DISTRO-rviz
具体如下【Ubuntu18.04 ROS版本melodic】：
sudo apt-get install ros-melodic-rviz
此外，安装以下发布机器人关节状态（例如角度）的软件包:
格式如下：
sudo apt-get install ros-$ROS_DISTRO-joint-state-publisher-gui
具体如下【Ubuntu18.04 ROS版本melodic】：
sudo apt-get install ros-melodic-joint-state-publisher-gui
为了在我们的视觉中有一个好看的机器人，而不仅仅是一堆漂浮在太空中的坐标框架，我们必须告诉Rviz我们的机器人看起来像什么。因此，我们想安装ROS工业项目的通用机器人包。这个软件包提供了大量的文件和功能，以便与通用机器人不同的机器人一起工作。其中包含了我们在ur_description包中寻找的视觉信息。因此，首先将通用机器人存储库克隆到您的工作区的src目录中。
~/hzx2_ws/src$ git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
如果您不使用git，您也可以下载存储库并将其提取到您的工作区中。接下来，我们必须确保安装了所有依赖项:
~/hzx2_ws/src$ cd ..
~/hzx2_ws$ rosdep update
~/hzx2_ws$ rosdep install --rosdistro melodic --ignore-src --from-paths src
然后构建您的工作区并激活工作区（如果默认情况下尚未完成此操作）:
/home/hzx/hzx2_ws$ catkin_make
// 该home/hzx/hzx2_ws路径下执行source
source devel/setup.bash
现在最好的方法是从ROS安装urdf_tutorial包。
sudo apt-get install urdf_tutorial
// 或者
sudo apt-get install ros-melodic-urdf-tutorial
我们将在本教程中使用的URDF文件 ur5_rviz.txt 存储在此链接下。将URDF的内容复制到文本文档中，并在您选择的文件系统位置保存为“ur5_rviz.urdf”。然后，显示机器人刚刚运行:
roslaunch urdf_tutorial display.launch model:='/home/hzx/hzx2_ws/src/universal_robot/ur_description/urdf/ur5_rviz.urdf'
或者您可以直接使用ur_description包提供的启动文件并运行以下两个命令:
// 此必须在工作空间下，启动后无变化
roslaunch ur_description ur5_upload.launch
// 在新的终端打开启动即可，启动后打开rviz并显示ur5形状
roslaunch ur_description view_ur5.launch
本教程将解释 urdf_tutorial 使用的自定义URDF和display.launch文件，因为ur5_description包在这些文件中提供了我们现在不需要的附加内容，但这两种方式在Rviz中的工作方式是相同的。
运行启动文件后，您将看到UR5机器人显示在Rviz中。很简单，对吧？除了Rviz之外，启动文件还会启动一个小GUI，让您单独移动机器人关节。这很有趣，但不幸的是，只是玩玩并不能教会我们很多东西，所以让我们看看幕后的URDF，了解它是如何工作的。
个人实操方案：
step 1、创建工作空间并创建新功能包：
// 创建工作空间
mkdir -p catkin_ws/src
cd catkin_ws
catkin_make
// 创建新的功能包 并 创建文件夹
cd catkin_ws/src
catkin_create_pkg ur5_demo urdf xacro
cd ur5_demo
mkdir -p urdf/urdf
mkdir launch
mkdir -p meshes/visual
mkdir -p meshes/collision
mkdir config // 先不用
// 填入下载的 universal_robot 功能包内的一些内容，具体形式见下图所示：
universal_robot/ur_description/meshes/ur5/visual/XXX.dae
universal_robot/ur_description/meshes/ur5/collision/XXX.stl
将下载的 ur5_rviz.txt 内容填入 ur5_rviz.urdf
记得修改当前 ur5_rviz.urdf 中的所有的 filename="package:// 后面的路径，改成自己的路径
// 例如我的修改如下：
<geometry>
<mesh filename="package://ur5_demo/meshes/visual/base.dae"/>
</geometry>
step 2、创建launch文件
// display_demo.launch 填入如下内容：
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find ur5_demo)/urdf/urdf/ur5_rviz.urdf" />
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" />
</launch>
step 3、启动 launch 文件
hzx@vm:~/Music/catkin_ws$ roslaunch ur5_demo display_demo.launch
// 此步骤若出现问题，可以在 catkin_ws路径下，先执行catkin_make 再执行 source devel/setup.bash，最后执行roslaunch
// roslaunch正常启动后，需要进行一些设置：
// —— Rviz大界面 的左下角点击【 Add】
// —— 在新出的 Rviz小界面 选择 【By display type】 中的 【RobotModel】
// —— 再修改 Rviz大界面的【Fixed Frame】的值为：base_link，记得在此处回车使得base_link生效；
// 就出现如下的效果了。
// 为了避免每次都设置Rviz界面，可以将设置好的Rviz界面保存为urdf.rviz ，在launch文件中直接调用此文件就好了。
// —— 首先 在设置好的Rviz界面【如上图所示】点击 File
// —— 点击 Save Config As
// ——选择路径 /catkin_ws/src/ur5_demo/config/ 并填写文件名 urdf.rviz ，点击Save。
// —— 可以看到该路径下 /catkin_ws/src/ur5_demo/config/ 有了名为 urdf.rviz 的文件。
// 将该文件路径写入launch文件，具体如下：
// —— 打开 display_demo.launch 文件，修改其内容：
// —— 由 <node name="rviz" pkg="rviz" type="rviz" />
// ——改为 <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur5_demo)/config/urdf.rviz" required="true"/>
// 解释一下：args—— 将“XXX”内的参数传递给节点，此参数指明了需要启动的Rviz文件的路径；
// required —— 表明该节点是否必须，如果为 true,那么如果该节点退出，将杀死整个 roslaunch
step 4、结束 over
3.2 The URDF file
具体URDF等学习教程可参照 此教程
如果您之前没有将URDF复制到您的文件系统中，您可以在此处查看完整的文件，跟随我们浏览URDF的结构。它使用XML格式，由填充信息的不同标签组成。我们要描述的机器人由提供基本文件结构的 连杆/link 和 关节/joint 组成:
<robot name=”ur5”>
<link name = “...”
…
</link>
<joint name = “...”>
…
</joint>
...
</robot>
我们现在要告诉Rviz的是 连杆/link 的外观 。可以通过直接在URDF中定义一些几何图形来创建您自己的自定义机器人（详细信息请参见本教程），但是由于我们的UR5机器人已经存在并有良好的文档记录，因此我们只需使用一个描述其外观的文件 .dea，见【/universal_robot/ur_description/meshes/ur5/visual/base.dae】。该文件包含在ur_description包中，并直接从机器人的CAD模型中导出。请注意，这仅用于描述其视觉外观。
作为一个例子。肩关节的.dea文件包括如
<link name="shoulder_link">
<visual>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/visual/Shoulder.dae"/>
</geometry>
</visual>
</link>
关于 关节/joint，我们首先需要定义它们的类型。UR5的旋转接头有一定的限制，属于“revolute”类别。关节的目的是连接和移动连杆，因此在< joint>标记中我们指定了一个父连杆和一个子连杆。< origin>和< axis>标签描述了机器人运动学。
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
<axis xyz="0 1 0"/>
<limit effort="150.0" lower="-3.14159265" upper="3.14159265" velocity="3.15"/>
</joint>
关节描述值得更详细地研究，因此让我们以Rviz为辅助来深入了解正在发生的事情。
3.3 Robot Kinematics
机器人运动学
为了可视化机器人运动学，我们在每个关节的中心想象一个坐标系。为了描述运动链，我们简单地从基坐标系开始，看看它在到达最后一个关节之前是如何在机器人中传播的。从一个坐标系到下一个坐标系可能会发生两种变化。【1、它可以围绕自己的一个轴旋转】，如我们在左图中看到的用颜色编码的x轴、y轴和z轴。【2、它的原点可以从一个关节移动到下一个关节】，如右图中的箭头所示（在这种情况下，箭头指向相反的方向，但您可以获得概念）。
这种从一个关节到下一个关节的传播就是URDF中所描述的。作为示例，让我们再次查看 shoulder pan joint 和 shoulder lift joint 及其相应的父连杆和子连杆。
Coordinate frames in the base and shoulder joints
我们从位于机器人 base_link 底部的世界系开始。这是机器人与世界连接的地方，也是我们运动链的起点。为了到达 shoulder pan joint 中心的第二个坐标系，我们必须沿着z轴移动该坐标系。这正是< origin >和< axis >标记中定义的内容，1为沿着该轴，0为不沿着该轴。
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
<axis xyz="0 0 1"/>
xyz 属性指的是关节之间坐标系的线性移动。URDF显示第一坐标系必须沿z轴移动0.089159(米)才能到达第二坐标系，而其他轴不变。轴标签定义关节在操作过程中围绕哪个轴旋转(在本例中为z轴)。请注意，这是在坐标系传播之后。
现在，为了进入第三坐标系，还有更多的事情要做。除了沿y轴移动，我们还必须旋转坐标坐标系。这包含在 shoulder lift joint 的描述中：
<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
<axis xyz="0 1 0"/>
rpy属性代表滚动、俯仰和偏航。这些命名约定定义如下:
滚动:绕x轴旋转
俯仰:绕y轴旋转
偏航:绕z轴旋转
URDF 告诉我们径向旋转角度，因此为了到达第三个坐标系，我们必须绕y轴旋转1.570796325弧度（90度）。这个方案一直持续到我们在最后一个机器人关节处的最后一坐标系结束。
我希望这有助于澄清机器人的结构——其运动学——在URDF中是如何描述的。对于UR5，这是通用机器人定义的零配置姿势。通过将所有关节角度设置为零位置，并像我们对肩关节所做的那样逐个检查关节，您可以理解完整机器人的这种方法。
现在我们知道如何在一个简单的文件中描述一个机器人，这样我们就可以看着它并虚拟地玩它。这种设置非常适合为我们的机器人开发运动规划器之类的东西。然而，除了可视化我们的机器人之外，我们还希望模拟它在物理定律发挥作用的真实世界中的行为。这就是为什么在下一个教程中，我们将看看如何扩展我们的URDF用于功能强大的模拟工具Gazebo！你也可以在这里找到所有教程的概述！
四、ROS Tutorial: Simulate the UR5 robot in Gazebo – URDF explained【Basics】
ROS教程四： 在Gazebo中仿真UR5机器人—解释 URDF
今天的目标是在Gazebo安装我们的ur5机器人。Gazebo是一款流行的模拟工具，它不仅可以让你像在Rviz中一样可视化你的机器人，而且还内置了一个物理引擎。这样你就可以在更真实的环境中测试你的机器人应用程序。我们将看看我们需要添加到机器人描述中的属性-我们的order以便模拟它以及如何在Gazebo中生成我们的机器人。
注意： 如果您只想让UR5机器人在Gazebo中运行，而不了解URDF中定义的属性以及如何将URDF描述连接到Gazebo环境，那么您可以跳到快速入门部分。在那里，您可以获得在Gazebo中快速生成UR5所需的所有命令。
4.1 Gazebo
如果您还没有安装ROS，请按照ROS wiki上的说明进行安装。如果您缺少Gazebo（如果您选择完全安装ROS，它应该已经安装），只需在Linux终端中运行以下命令:
curl -sSL http://get.gazebosim.org | sh
这已经允许我们生成Gazebo环境，例如通过运行:
roslaunch gazebo_ros empty_world.launch
当然，一个空旷的世界是很无聊的，所以让我们把机器人放进去吧。我们使用Rviz的前一个教程已经浏览了UR5机器人的描述文件以了解URDFs。如果您对学习URDFs的基础知识感兴趣，请查看之前的 教程三。
Gazebo可以使用URDF描述文件来模拟我们的机器人。在gazebo下，它将URDF转换为另一种更适合模拟的格式SDF。然而，为了使gazebo能够与我们的URDF一起工作，我们需要添加一些代码行。Gazebo需要一些额外的属性，这基本上是影响机器人移动方式的所有属性，以便正确进行模拟。你可以在这里找到与gazebo一起使用的改进型URDF。在那里，您可以跟随我们详细讨论添加的属性。
4.2 Adding intertia
添加中间体
Gazebo必须知道机器人部件（即连杆）的重量及其惯性，这是衡量该重量如何分布的标准。该信息应包含在< link>内的URDF中，并位于< visual>和< collision>部分之后。这是基础关节的一个示例:
</collision>
<inertial>
<mass value="2.275"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
<inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
</inertial>
对于三维旋转的刚体，在 < inertia> 标签中定义的一般惯性矩阵如下所示:
矩阵元素定义了惯性力矩。例如，Ixx定义绕x轴的惯性力矩，而Ixy定义惯性力矩的xy乘积。更详细的讨论超出了本文的范围，但是如果您感兴趣的话，可以在google上搜索更多相关信息。此外，我们必须知道的唯一事情是矩阵对角线上方的元素等于其下方的元素，这意味着Ixy=Iyx，Ixz=Izx和Iyz=Izy。这就是为什么我们只需定义3×3旋转惯性矩阵的六个元素。
< origin> 标签分别定义了我们连杆的质心或重心。这是我们系统中所有部件的平均位置，这些部件根据它们的质量进行加权。在计算加速度时，这是一个有用的属性。
4.3 Collision properties
碰撞属性
为了进行适当的模拟，Gazebo必须知道机器人何时会撞到东西，尤其是它自己。因此，我们需要它告诉我们连杆的几何形状。为了简单起见，我们使用.stl 文件直接来自我们机器人的CAD模型，仅描述了连杆几何形状，没有任何颜色或材料的定义。通常，通过定义近似几何形状或降低模型的分辨率来简化这些详细的几何模型，以便更快地计算碰撞。
<collision>
<geometry>
<mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl"/>
</geometry>
</collision>
4.4 Limits and dynamics
约束和动力学
此外，我们将为关节特性定义一些限制。这必须添加到< joint>标记中。
<limit effort="28.0" lower="-3.14159265" upper="3.14159265" velocity="3.2"/>
<dynamics damping="0.0" friction="0.0"/>
effort 决定了可以施加到机器人上的最大力（或扭矩），这主要取决于电机的功率有多大。通过下限和上限，我们定义了关节可以在径向移动的范围。通过限制最大速度，我们限制了单个关节的移动速度。在 < dynamics> 标签中，阻尼和摩擦力是进一步定义关节动力学的附加参数。我们现在将它们设置为零，因为我们没有它的准确数据，但是为了使您的模拟更加真实，您可能希望在您的URDF中使用它们。
请注意， 限制和动力学的定义是可选的，不是使用带Gazebo的URDF的先决条件。
4.5 Tying the robot to the ground (A fixed world joint)
将机器人固定在地面上（固定的世界关节）
最后一步，为了将机器人与我们的gazebo环境连接起来，我们在URDF上增加了一个额外的关节。这个关节——我们称之为世界关节world joint——是固定的，它的唯一目的是将我们的机器人固定在模拟世界中。这就是为什么我们定义world为父连杆，base_link为子连杆。
<link name="world"/>
<joint name="world_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<!-- TODO: check base_link name of robot -->
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</joint>
由于我们现在添加了Gazebo要求的所有内容，因此我们可以生成机器人。首先，将 ur5_gazebo.txt 文件的内容复制到ur5_gazebo.urdf 文件中，txt文件地址在此 ，并将ur5_gazebo.urdf 存储在文件系统中的任何位置。其次，如果您没有安装上一教程中的 ur_description包，请运行以下命令，并一如既往地将《distro/发行版》替换为您的ROS版本:
~/hzx2_ws/src$ git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
~/hzx2_ws$ rosdep update
~/hzx2_ws$ rosdep install --rosdistro melodic --ignore-src --from-paths src
然后构建您的工作空间:
~/hzx2_ws/src$ cd ..
~/hzx2_ws$ catkin_make
// 在/home/hzx/hzx2_ws/src/路径下
~/hzx2_ws$ source devel/setup.bash
现在让我们像以前一样在gazebo中创建一个空世界:
// source $HOME/catkin_ws/devel/setup.bash后，此步骤一定要先执行，待到出现一个空的rviz出现，
roslaunch gazebo_ros empty_world.launch
在新的终端中，我们可以使用spawn_model命令生成机器人:
// 待到出现一个空的rviz之后，在任意终端执行此命令，空的rviz中出现机械臂。
rosrun gazebo_ros spawn_model -file /home/hzx/hzx2_ws/src/universal_robot/ur_description/urdf/ur5_gazebo.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
若不先执行【生成空rviz】的命令，而先执行生成机器人的指令，会出现如下错误：一直卡在这里
我们将URDF文件的路径作为参数和机器人应该产生的坐标传递给它。我们为z轴定义了一个0.1的小偏移量，因为在我们的空白世界的正中心，有一个接地板，它也出现在模型的左侧。这块板是看不见的，但当我们试图将机器人精确地放置在同一位置时，它会引起碰撞。
在上图中，为了更好的可视性，阴影被禁用。您可以通过在“world”选项卡中单击“Scene”并从“shadows”选项中删除勾号来完成此操作。当你现在看着gazebo里的机器人时，它可能看起来有点可怜，只是无力地躺在地上。 这是因为我们现在无法控制机器人上的任何东西，所以模拟中发生的所有事情都是机器人定义了一堆重量，关节中没有力或摩擦力，只是由于重力而落到地面。 这就是为什么我们需要机器人控制器来移动机器人并做一些很酷的事情。在下一篇文章中，我们将添加ros_control包并设置一些不错的位置控制器来移动我们的关节。你可以在这里找到所有教程的概述。
个人实操方案：
step 1、创建工作空间并创建新功能包：
// 创建工作空间
mkdir -p gazebo_urdf_ws/src
cd gazebo_urdf_ws
catkin_make
// 创建新的功能包，导入依赖 并 新建文件夹
cd gazebo_urdf_ws/src/
catkin_create_pkg gazebo_urdf urdf xacro gazebo_ros gazebo_ros_control gazebo_plugins
cd gazebo_urdf
mkdir -p urdf/urdf
mkdir launch
mkdir -p meshes/visual
mkdir -p meshes/collision
mkdir config
// 填入下载的 universal_robot 功能包内的一些内容，具体形式见下图所示：
universal_robot/ur_description/meshes/ur5/visual/XXX.dae
universal_robot/ur_description/meshes/ur5/collision/XXX.stl
将下载的 ur5_gazebo.txt 内容填入 ur5_gazebo.urdf
记得修改当前 ur5_gazebo.urdf 中所有的 filename="package:// 后面的路径，改成自己的路径
// 例如我的修改如下：
<geometry>
<mesh filename="package://gazebo_urdf/meshes/visual/base.dae"/>
</geometry>
step 2、创建launch文件
<launch>
<!-- 将 Urdf 文件的内容加载到参数服务器 -->
<param name="robot_description" textfile="$(find gazebo_urdf)/urdf/urdf/ur5_gazebo.urdf" />
<!-- 启动 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 在 gazebo 中显示机器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model ur5 -param robot_description -x 0 -y 0 -z 0.1" />
</launch>
// 解释一下：
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model ur5 -param robot_description -x 0 -y 0 -z 0.1" />
// args 内的 ur5 为 ur5_gazebo.urdf文件中robot的name，-z 指明了该模型的初始位置将在Z轴上沿正方向移动 0.1 m。其余基本为固定格式。
// 可以设置不同的 xyz的值启动launch文件，查看模型在gazebo中的状态。
step 3、启动 launch 文件
hzx@vm:~/Music/gazebo_urdf_ws$ roslaunch gazebo_urdf gazebo_urdf.launch
// 此步骤若出现问题，可以在 catkin_ws路径下，先执行catkin_make 再执行 source devel/setup.bash，最后执行roslaunch gazebo_urdf gazebo_urdf.launch
补充一点：在终端执行Ctrl + C 或者 Ctrl +Z 关闭gazebo界面时，会出现gazebo界面无法关闭的情况，如下图所示：
//此时的解决方法是终端执行
hzx@vm:~/Music/gazebo_urdf_ws$ killall gzserver && killall gzclient
// 就可以正常关闭gazebo界面了。
step 4、结束 over
五、ROS Tutorial: Control the UR5 robot with ros_control – How to tune a PID Controller【Control】
ROS教程五： 使用ros_control控制UR5机器人–如何调整PID控制器
在 教程四 中，我们在Gazebo中设置了UR5机器人，并意识到我们需要一些控制器来移动我们的机器人。Ros_control 是一个帮助控制器实现和硬件抽象的包。这是一种为我们的关节轻松设置低级控制的简便方法。在本教程中，我们将看到如何安装ros_control，使用每个关节的位置控制器扩展URDF描述，并使用配置文件对其进行参数化。我们还将研究一种简单的技术来调整位置控制器的参数。我们走吧！
注意： 如果您只想在Gazebo中运行UR5机器人和ros_control来控制关节位置，而不了解如何使用ros_control设置和配置关节位置控制器并调整它们以获得良好的控制性能，那么您可以直接跳到快速入门部分。在这里，您可以找到快速入门所需的所有命令。否则，让我们开始设置我们的联合控制器吧！
5.1 Setting up ros_control
要继续操作，请将带有修改后的URDF、配置文件和启动文件的git存储库克隆到您的 catkin_ws 工作区并构建工作区。
~/hzx2_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/hzx2_ws/src$ cd ..
~/hzx2_ws$ catkin_make
~/hzx2_ws$ source devel/setup.bash
如果您不熟悉什么是工作区以及如何构建ROS包，请从 本教程 开始。如果您不使用git，也可以下载。来自github的zip文件（链接）并将其解压缩到您的工作区文件夹中。然后您可以构建工作空间。
请注意，如果您没有将源命令添加到./bashrc文件中，您必须为您启动的每个新linux终端重复最后一个命令，以便ROS找到包的位置。
要安装ros_control和某些控制器，请在终端中运行以下命令:
sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers
5.2 Modifying the URDF
修改URDF 具体内容讲解可以参考此处
简要介绍：
ros_control 内按照流程 分为四大部分，即：
controller
硬件资源
硬件抽象
实体
【controller 关节指令】利用【硬件资源的 joint command 接口】——>【硬件抽象内，通过约束等限制处理后，通过 effort transmission接口】——>write ——> 【实体的执行器】
【实体的关节状态】 ———> read ——>【硬件抽象内的 state transmission】——> 【资源硬件内的 joint state 接口】——>【控制器】
controller manager 则负责管理controller 的生死，包括：控制器的加载、卸载、启动、停止等。
Transmission：将关节指令转换成实体执行器的控制信号，每个关节均需要配置相应的传动系统，可在urdf文件内配置。
controller：负责计算出关节指令，负责计算实现目标所需的关节指令。
硬件抽象接口：命令执行器移动；从关节传感器获得关节状态反馈。
硬件抽象内包含：关节约束、力矩转换、状态转换
实体上：有控制关节运动的执行器，和反馈关节状态的传感器。
我现在将描述必须添加到URDF的部分，以便与ros_control一起使用。如果您下载了存储库，这些修改已经在urdf文件夹中的 ur5_jnt_pos_ctrl.urdf 文件中完成。首先，我们需要插入一个ros_control插件，该插件直接在开始的< robot>标签之后解析URDF。
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
其次，我们必须在< link>和< joint>规范之后添加位置控制器。这是在< transmission>标签内完成的，该标签显示在下方的 for the soulder lift joint 中:
<transmission name="shoulder_lift_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_lift_joint">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_lift_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
< type >标签让我们定义执行器的类型。我们选择SimpleTransmission 作为传输接口，它代表一个简单的减速器传输。这正是我们在制动器侧(电机)和关节侧(连接连杆的输出轴)之间的情况，其中减速由齿轮箱定义。
为了在Gazebo中使用我们的控制器，我们定义了一个EffortJointInterface，因为最终，我们的位置控制器输出和关节电机上命令的值是作用力(或力)。我们假设执行器和关节之间的传输比为1:1，因此我们将< mechanicalReduction >属性设置为1。
5.3 Adding the configuration file
添加配置文件
除了修改URDF之外，我们还必须提供一个配置文件，将控制器参数加载到参数服务器。我们将使用的位置控制器是PID控制器，这意味着它由一个比例项、一个积分项和一个微分项组成。如果您不熟悉这些东西，请不要担心，我们将在本文的最后一节中更详细地讨论它们，届时我们将调整我们的控制器。
现在需要理解的重要事情是， 我们需要一个配置文件来告诉 ros_control 我们的关节控制器的参数。配置文件存储在项目文件夹中名为 config 的单独子文件夹中。这是我们的UR5机器人的文件:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
shoulder_pan_joint_position_controller:
type: effort_controllers/JointPositionController
joint: shoulder_pan_joint
pid: {p: 500.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
shoulder_lift_joint_position_controller:
type: effort_controllers/JointPositionController
joint: shoulder_lift_joint
pid: {p: 500.0, i: 100.0, d: 30.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
elbow_joint_position_controller:
type: effort_controllers/JointPositionController
joint: elbow_joint
pid: {p: 10000.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_1_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_1_joint
pid: {p: 200.0, i: 10.0, d: 20.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
wrist_2_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_2_joint
pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
wrist_3_joint_position_controller:
type: effort_controllers/JointPositionController
joint: wrist_3_joint
pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
为每个关节控制器定义了p、I和d值。在顶部还有一个机器人状态发布器。那个发布联合状态，现在对我们来说不重要。
5.4 The launch file
启动文件
在我们开始一切之前，让我们快速浏览一下我们项目的启动文件。
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find ur5-joint-position-control)/config/ur5_jnt_pos_ctrl.yaml" command="load"/>
<param name="robot_description" textfile="$(find ur5-joint-position-control)/urdf/ur5_jnt_pos_ctrl.urdf"/>
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="shoulder_pan_joint_position_controller shoulder_lift_joint_position_controller elbow_joint_position_controller wrist_1_joint_position_controller wrist_2_joint_position_controller wrist_3_joint_position_controller joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/ur5/joint_states" />
</node>
</launch>
首先，我们将控制器参数从 ur5_config.yaml 和URDF加载到参数服务器。其次，我们用 ros_control的controller_spawner 生成联合控制器。然后我们启动robot_state_publisher，它为Rviz（不重要的atm）等其他程序将joint _ state _ controller transformer的关节状态主题发布到 /tf 。).
在开始之前， 我们必须做的最后一件事是从ros工业项目中获取 universal_robot 包，其中包含 ur5 机器人的视觉和几何描述文件。如果您完成了前面的教程之一，您可能已经安装了它。否则，请执行以下命令并用您的ROS版本替换< distro>:
~/hzx2_ws/src$ git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
cd /home/hzx/hzx2_ws/
~/hzx2_ws$ rosdep update
~/hzx2_ws$ rosdep install --rosdistro melodic --ignore-src --from-paths src
然后编译您的工作空间:
~/hzx2_ws$ catkin_make
~/hzx2_ws$ source devel/setup.bash
5.5 Launching the UR5 in Gazebo with ros_control
使用ros_control在Gazebo中发布UR5
现在让我们 先启动 Gazebo:
// 工作空间路径下
~/hzx2_ws$ roslaunch gazebo_ros empty_world.launch
然后，在两个新的终端中，生成机器人并启动控制器:
// 任意路径下均可
~/hzx2_ws$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
// 要在工作空间下：执行source后执行，否则报错。
~/hzx2_ws$ roslaunch ur5-joint-position-control ur5_joint_position_control.launch
机器人在它的零配置下生成，对于UR5来说，这意味着它“躺”在地上。然而，从那里开始调整控制器可能有点尴尬。你首先要让机器人进入一个合适的位置，在这个位置上你可以控制整定所需的位置。对于未整定的控制器，这可能很烦人。
为了避免这种情况， 我们可以使用 spawn 命令来定义机器人的初始姿态。但是，Gazebo的这一功能似乎仍然存在一些bug。让我来描述在某个适合我的关节配置中生成机器人的步骤:
首先，像以前一样启动Gazebo:
~/hzx2_ws$ roslaunch gazebo_ros empty_world.launch
然后 按下下方栏中的暂停按钮【Real Time Factor之前的按钮】暂停仿真【暂停后标志为三角形】。
之后，使用以下参数生成机器人:
~/hzx2_ws$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5 -J shoulder_lift_joint -1.5 -J elbow_joint 1.0
此刻，机器人出现，并立着。
附加参数 -J shoulder_lift_joint -1.5 and -J elbow_joint 1.0设置肩关节\the shoulder lift和肘关节\the elbow joint的初始角度。
然后我们像以前一样启动控制器:
~/hzx2_ws$ roslaunch ur5-joint-position-control ur5_joint_position_control.launch
之后，我们可以通过 单击下方栏中的播放按钮 来 取消仿真暂停 。现在机器人处于一个更实际的位置，控制器试图将机器人保持在那个位置。
点击取消仿真暂停时，机械臂会出现轻微的晃动。
您还可以运行下面的启动文件，它会为您执行前面描述的步骤(除了在启动后卸载Gazebo):【没测试】
roslaunch ur5-joint-position-control ur5_gazebo_position_control.launch
5.6 Commanding the joint positions
控制关节位置
现在我们可以向位置控制器发送位置命令来移动机器人。如果你不知道发布主题和订阅主题的概念，请阅读ROS wiki中的教程。位置控制器监听主题 ur5/controller_name/command/。发送命令的最简单方法是启动rqt_gui:
// 任意一个终端下
$ rosrun rqt_gui rqt_gui
在“插件\Plugins”下的菜单栏中，我们从“主题\Topics”文件夹中选择“消息发布者\Message Publisher”。
现在我们可以从列表中选择主题并在上面发布。在Topic旁边的上方选择栏中，您可以简单地键入主题名称，然后按绿色加号按钮添加它们。我们将完成 wrist_1 joint 的调整过程，因此请添加 /wrist_1_joint_position_controller/command 主题。我们可以通过扩展要发布的主题来设置一个静态值，并在主题的data属性下的expression列中输入一个值。
另一种可能性是定义函数，让rqt为我们计算一个序列。我们将编写类似sin（0.1 * I）的表达式来代替静态值，其中I是由rqt提供的时间变量。我们可以配置的另一个属性是主题发布的速率。我们现在将它设置为100。只要您在主题左侧的框中打勾，您就应该看到关节对您的命令做出反应。
注意：启动rqt_gui 后，选中话题后，需要在expression 设置某关节控制器的控制指令，此为弧度制。
1、要记得给话题打勾，打勾的才会执行。
2、gazebo界面下部的暂停按钮要是继续状态，即两个竖杠，如上图左下部所示。
不同关节之间的控制器性能可能会有很大差异，因为我们已经为具有不同移动权重的不同关节设置了默认控制器参数。
5.7 Tuning a PID Position Controller
整定PID位置控制器
下面我们可以看到一个调节不好的控制器和一个调节良好的控制器之间的区别，两者对相同的位置命令做出反应。你可以看到，我们需要调整控制参数，以使机器人平稳移动。这里，rqt_gui 再次派上了用场。
像之前使用消息发布器一样，将插件菜单中的绘图插件\Plot plugin 添加到rqt窗口。
与 topic 发布器类似，您可以通过在选择栏中搜索主题并按绿色加号，将想要可视化的主题添加到图表中。对于整定调优，我们想看看控制器命令和系统对它的响应。joint_state_controller控制器发布的是机械臂所有关节的当前状态，而每个关节对应的控制器通过 /关节控制器/state 话题来发布此关节的当前状态，如/wrist_1_joint关节发布的状态话题为/wrist_1_joint_position_controller/state。我们可以通过订阅此话题，来了解/wrist_1_joint关节的当前状态。 添加到绘图中。调整控制器参数的一种常见方法是查看阶跃响应，即当我们在命令值中设置一个突然的阶跃时，控制器和系统如何表现。
我们可以简单地通过改变 /wrist_1_joint_position_controller/command 的值来实现，例如从0到-1，在 wrist_1 joint 的情况下，这意味着向上移动。现在我们可以观察机器人的真实位置是如何跟随我们的命令的。让我们来看看增强控制器性能的调整过程。
Rqt 让我们直接控制控制器参数。只需从配置文件夹下的插件菜单中添加动态重新配置，就像绘图和消息发布程序一样，并通过单击pid从左侧列表中选择 wrist_1_joint_positon_controller 。
PID 控制器输出τ（施加在机器人关节上的力）的计算如下所示:
比例部分
我们控制器的比例部分只是简单地乘以误差perror
在期望位置和当前位置之间使用参数Kp。因此，如果我们增加Kp，控制器会更快、更准确地到达目标位置，但振荡会增加。此外，如果只有Kp增益，我们将需要非常高的增益才能达到精确的目标位置。为了演示不同增益的影响，让我们在动态重新配置插件中设置p = 20，i = 0.0，d = 0.0。现在我们命令位置步长从0到-1。
微分部分
微分部分（d）用于系统的阻尼。它乘以速度v
我们的关节（即位置随时间的变化）与Kd参数，并从我们的控制器输出中减去它。当Kd增加到例如5时，我们看到控制器花费更多时间到达目标位置，但几乎没有超调。根据经验，在许多控制应用中，10%的超调是可以接受的。这意味着0.1弧度的过冲是没有问题的。
积分部分
在积分项中，我们简单地将一段时间内的测量误差相加（∫t0perror（t））使得随着时间的推移，即使很小的误差也会对控制输出产生重大影响。这样我们就消除了比例部分无法处理的持久位置误差。然而，积分也会导致更多的超调。
其他参数（即i_clamp_min和i_clamp_max）限制了积分项可以达到的幅度，使其不会产生不必要的高扭矩。让我们设置i = 0.1，i_clamp_min = -200和i_clamp_min = 200。
您可以继续使用不同的控制器，直到为我们的UR5机器人找到合适的设置。对于某些关节来说，这可能相当具有挑战性，但这是一种很好的方式来感受机器人属性和控制参数对控制器性能的影响。
我们将在下一个教程中使用调整后的参数，在下一个教程中，我们希望通过使用正向和逆向运动学在笛卡尔空间中控制机器人的tcp（tool center point i.e. the tip of the robot arm，工具中心点，即机器人手臂的尖端）。这对于几乎每个机器人手臂应用来说都很重要，以控制连接到机器人手臂的工具的位置。如果你准备好接受挑战，那么让我们继续下一个 教程六。如果你想让你的生活更轻松，并直接控制机器人的运动规划框架请开始 教程七 。在这里您可以找到所有教程的概述。
六、Tutorial: Control the TCP position of a UR5 robot in C++ with KDL – Inverse Kinematics explained【Control】
教程六： 用KDL在C++中控制UR5机器人的TCP位置–逆向运动学解释
在之前的教程中，我们命令机器人关节的位置。然而，机械臂的最终目标是 控制工具中心点（tcp） ，在该点上我们可能有一个附着的 抓取器 或任何其他能够操纵我们环境的 工具。
我们想要命令tpc到空间中的特定位置，更具体地说，在笛卡尔空间中。但是我们如何从控制单个机器人关节到直接控制tcp位置呢？这就是正向和逆向运动学概念的由来。
为了真正掌握它，我们将实现一个C++程序，允许我们在笛卡尔空间中移动tcp。
我们将创建一个ROS节点，使用KDL进行运动学计算，并与gazebo中的模拟机器人进行交互。 我们将首先运行程序，然后一步一步地检查代码。在帖子的最后，我们将详细讨论逆向运动学，并看看它是如何在KDL库中实现的。让我们开始吧！
6.1 Clone, build and execute the TCP Position Controller
克隆、构建和执行TCP位置控制器
在之前的教程中，我们已经为UR5关节和Gazebo中的机器人模拟调整了ros_control包中的位置控制器。如果你有兴趣并且还没有这样做，你可以先浏览一下教程。否则，您需要的所有文件—包括(粗略)调整的关节控制器参数—都在存储仓库中，因此您不必担心。
首先， 确保您已经安装了Gazebo和ros_control，这两个工具我们已经在之前的教程中使用过了。
$ curl -sSL http://get.gazebosim.org | sh
$ sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers
如果您尚未设置catkin_ws 工作空间，请进行设置。
第二， 我们需要 the universal robots package。同样，如果您完成了之前的教程之一，这应该已经存在。否则我们需要从源代码安装它。将存储仓库克隆到您的 catkin 工作空间的源文件夹中，并检查依赖性:
~/hzx2_ws/src$ git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
~/hzx2_ws/src$ cd ..
~/hzx2_ws$ rosdep update
~/hzx2_ws$ rosdep install --rosdistro melodic --ignore-src --from-paths src
现在，继续将以下两个存储库克隆到catkin_ws 工作空间的src文件夹中（如果您完成了上一篇关于关节控制器的教程，您可能已经克隆了第一个存储库）:
~/hzx2_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/hzx2_ws/src$ git clone https://github.com/dairal/ur5-tcp-position-control.git
如果您不习惯使用git，也可以从以下链接下载zip文件： 关节位置控制 和 tcp位置控制 。之后，只需将它们解压缩到您的catkin工作区的src文件夹中。
然后 构建您的工作空间:
~/hzx2_ws/src$ cd ..
~/hzx2_ws$ catkin_make
~/hzx2_ws$ source devel/setup.bash
这将构建我们的 “ tcp position controller node”。我们将用于启动机器人的启动文件确保我们以“ paused mode ”启动Gazebo，并以定义的初始位姿生成机器人。为此并启动关节位置控制器，运行:
~/hzx2_ws$ roslaunch ur5-joint-position-control ur5_gazebo_joint_position_control.launch
若打印出现如下WARN：
根据所报警告【6处WARN】修改即可，修改如下图【共计6个】：
找到该路径下的urdf文件， ~/hzx2_ws/src/ur5-joint-position-control/urdf/ur5_jnt_pos_ctrl.urdf，
修改后记得重新catkin_make【可以先删除devel 和 build后再catkin_make】
// 修改前
<hardwareInterface>EffortJointInterface</hardwareInterface>
// 修改后
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
若出现如下WARN
[WARN] [1715485796.583586, 0.000000]: wait_for_service(/controller_manager/load_controller): failed to contact, will keep trying
可以修改 /ur5-joint-position-control/launch/路径下的 ur5_joint_position_control.launch文件中的
<node name=“controller_spawner” pkg=“controller_manager” type=“spawner” respawn=“false”
将respawn= “false” 设置为 respawn= “ture”
加载所有内容后，您将看到UR5机器人躺在地上，模拟处于暂停状态【因为在 ur5_gazebo_joint_position_control.launch 文件中，提前设置了 < arg name=“paused” value=“true” /> ，就是说开启暂停】。
当您取消暂停模拟【点击Rviz界面底部的小三角成为两竖杠】时，您将看到机器人移动到ur5_gazebo_pose.launch文件中定义的初始姿势【机械臂会出现轻微的晃动】。现在，打开新的终端并进入工作区间，运行以下命令启动我们之前构建的tcp位置控制器节点：【该节点是为了给定位置，控制手臂运动。】
~/hzx2_ws$ rosrun ur5-tcp-position-control tcp_position_controller
太好了，现在程序将 打印出你的机器人的当前tcp位置， 并要求你定义你想要在每个坐标轴的方向上移动多远，以及在哪个时间段内。运动从0.2这样的小数字开始，因为命令的单位是米，而我们机器人的工作空间是有限的。此外，我们的运动学解算器可能会因为某些目标位置而陷入麻烦。时间属性希望您以秒为单位定义运动的时间范围。
这是用于控制正在运行的UR5机器人的tcp位置的ROS节点
如您所见，控制器的性能仍远未达到最佳状态，但它确实有效！如果你玩这个程序，你可能会发现有些目标位置比其他位置更好，而有些根本不起作用。有时机器人甚至会发疯并自行崩溃（在这种情况下，您应该关闭一切并通过运行启动文件和启动节点重新启动）。这可能是因为运动学计算非常脆弱，或者因为期望的目标位置在机器人工作空间之外。现在让我们看看所有这些是如何用代码实现的。
6.2 The Code: Functionality and Interface
代码：功能和接口
注意： 这里描述的方法只是控制机械臂tcp位置的一种非常简单的方法。这个简单的实现旨在理解正向和逆向运动学的基本概念。
下面，我将重点介绍代码的重要部分。显示完整的文件对于一篇帖子来说太多了，它包含一些乏味的boiler plate代码，但是您可以在克隆的 ur5-tcp-position-control.git 存储库中找到源文件ur5_cartesian_position_controller.cpp，或者您可以通过 以下链接 查看其内容。
6.2.1 Publishing and Subscribing
发布和订阅
开始时，我们希望获得机器人的当前关节位置，以计算我们开始时的tcp位置。正如我们在上一个教程中看到的，我们的关节位置控制器主题/< name_of_the_joint>_position_controller/state 下发布该value。所以我们要做的是订阅每个位置控制器的process_value主题。以下代码片段显示了肩关节的外观:
ros::Subscriber shoulder_pan_joint_sub = n.subscribe("/shoulder_pan_joint_position_controller/state", 1000, get_shoulder_pan_joint_position);
在回调函数中，每当我们的订阅接收到新数据时都会调用该函数，我们将所有位置值存储在 jnt_pos_start 数组中。
const int Joints = 6;
KDL::JntArray jnt_pos_start(Joints);
void get_shoulder_pan_joint_position(const control_msgs::JointControllerState::ConstPtr& ctr_msg) {
jnt_pos_start(0) = ctr_msg->process_value;
}
稍后，当我们完成计算时，我们希望将位置命令发布到 topic 名称< joint _ name > _ position _ controller/command 上的所有关节，因此我们创建了六个发布，并将它们存储在 joint_com_pub 数组中，以便我们稍后可以轻松地迭代我们的关节命令。以下代码显示了数组初始化和肩部平移关节命令主题的指定。
ros::Publisher joint_com_pub[6];
joint_com_pub[0] = n.advertise<std_msgs::Float64>("/shoulder_pan_joint_position_controller/command", 1000);
6.2.2 Kinematic Solvers from the KDL library
运动学解算器 KDL库
KDL 库提供了一个用于建模和计算运动链的框架，这正是我们在给定关节角度的情况下找出当前tcp位置所需要的。为了能够进行这些运动学计算，KDL 需要提取机器人的运动学信息。在第109行，我们要求KDL从URDF创建一个树表示。URDF 是一个xml文档，它描述了我们的机器人的结构，我们在本教程中使用的文档在前面两个关于可视化【教程三】和模拟UR5机器人的教程【教程四】中有详细描述。从树形图中，KDL提取了第116行的运动链。为此，我们必须告诉函数应该在哪里开始，在哪里结束。在我们的例子中，我们需要从机器人 base_link 到最后一个wrist_3_link的运动链。
//Parse urdf model and generate KDL tree
KDL::Tree ur5_tree;
if (!kdl_parser::treeFromFile(urdf_path, ur5_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
//Generate a kinematic chain from the robot base to its tcp
KDL::Chain ur5_chain;
ur5_tree.getChain("base_link", "wrist_3_link", ur5_chain);
现在我们准备创建解算器。 我们有一个用于正向运动学的解算器，它根据关节位置计算我们的tcp位置，还有一个用于逆向运动学的解算器，它根据tcp位置计算我们的关节位置。 逆向运动学位置解算器也需要 vel_ik_solver 作为参数。
KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);
在本文的最后，我们将更深入地研究正向和逆向运动学是如何计算的，我还将解释我们作为求解者的参数提供的神奇数字。有了这些东西，我们就可以开始实际工作了。
使用正向运动学解算器，我们计算tcp位置。在行134中，解算器采用关节位置jnt_pos_start和一个空的 KDL::Frame tcp_pos_start 来存储得到的tpc姿态。一个 KDL::Frame很像坐标系，由它的方向和原点位置定义。
KDL::Frame tcp_pos_start;
fk_solver.JntToCart(jnt_pos_start, tcp_pos_start);
现在我们知道我们 开始的tcp位置，这是计算的 KDL:Frame的原点。我们将这些信息打印给用户，并询问他/她希望tcp如何移动。这发生在第142行的 get_goal_tcp_and_time() 函数中。作为额外的输入，我们以秒为单位获得运动的持续时间。根据x，y，z输入，我们可以通过简单地将用户定义的值添加到坐标中来计算目标位置 vec_tcp_pos_goal 的坐标系的原点。使用这个新的tcp位置并保持tcp_pos_start 方向。按照现在的情况，我们可以在第144行构建我们的目标坐标系的tcp_pos_goal。
get_goal_tcp_and_time(tcp_pos_start, &vec_tcp_pos_goal, &t_max);
KDL::Frame tcp_pos_goal(tcp_pos_start.M, vec_tcp_pos_goal);
现在，我们可以在第148行中调用逆向运动学解算器来计算新的tcp姿势的关节位置。
//Compute inverse kinematics
KDL::JntArray jnt_pos_goal(Joints);
ik_solver.CartToJnt(jnt_pos_start, tcp_pos_goal, jnt_pos_goal);
如您所见，解算器还将 初始关节位置jnt_pos_start作为输入 。稍后我们将讨论其原因。结果存储在jnt_pos_goal数组中，这就是运动学计算的全部内容！
6.3 Commanding the joints
控制关节
现在我们所要做的就是以一种有礼貌的方式命令我们的关节到达它们的新位置。我们使用最简单的方法，即在用户指定的时间范围内，在我们的起始位置和目标位置之间进行线性插值。
float compute_linear(double q_start, double q_goal, float t, float t_max) {
return((q_goal - q_start) * (t/t_max) + q_start);
}
在while循环的每个循环中，我们都会为关节控制器计算下一个位置命令。计算之间的时间步长由ROS节点的速率定义。在我们的例子中，节点以100 Hz运行，因此我们每10毫秒计算一次新的位置命令(第155行)。我们向行156中的每个关节位置控制器发布新值，休眠10毫秒并计算下一个时间步长，直到到达时间帧t_max的末尾。
while(t<t_max) {
std_msgs::Float64 position[6];
for(int i=0; i<Joints; i++) {
position[i].data = compute_linear(jnt_pos_start(i), jnt_pos_goal(i), t, t_max);
joint_com_pub[i].publish(position[i]);
}
ros::spinOnce();
loop_rate.sleep();
++count;
t += t_step;
}
这就是我们控制机器人tcp位置所需要的一切。你可能已经注意到这种方法有一些缺点。当我们命令关节时，我们不检查关节位置反馈，最后我们只是停止机器人，而不确保关节到达它们的目标位置。本质上，我们相信位置控制器能够紧密跟随我们的命令。这似乎对不太快的小运动是合理的。然而，对于更大和更快的运动，我们的关节水平控制器可能会 落后或超调， 特别是如果控制器参数不是很好地调整。
另一方面， 以线性方式计算中间位置命令不是命令真实机器人关节的最佳解决方案。通常人们会计算多项式， 以便能够例如定义运动开始和结束时的速度，并避免大的加速度。尽管如此，我们有一个非常简单的程序，允许我们控制机器人tcp的位置。它让我们专注于正运动学和逆运动学的概念。现在，我们将快速深入这些主题，这些主题是理解机器人如何操作的基础。我们也将看看KDL库如何实现必要的计算。不用担心，它并不像乍看起来那么复杂，我们不会让它过于复杂。
6.4 Forward/Inverse Kinematics in KDL
KDL正向/逆向运动学
那么我们如何从笛卡尔空间中给定的tcp位置计算关节位置呢？一般来说，有两种方法可以做到这一点：分析方法和数值方法。我们将只讨论数值方法，这是KDL和本教程中使用的方法。
在数值方法中，我们尝试一步一步地移动关节配置（角度）以接近tcp的期望目标位置。因此，我们试图减少一个误差函数。在我们的例子中，我们希望最小化当前tcp姿态和期望的tcp姿态之间的误差。tcp姿势（让我们称之为X
）由位置xx/y/z和方向xRx/Ry/Rz组成，总共有六个自由度。当我们有一个有n个关节的机器人时，每个自由度都取决于关节角度q1/…/n，即关节角度的函数（X = F（Q）).
如果我们假设X(一个6×1矩阵）是期望的tcp姿态，Q（一个nx1矩阵）是我们对关节位置的迭代猜测，δX是根据猜测得到的tcp姿态与我们期望的姿态之间的距离，我们可以将误差函数写成:δX = X–F（Q）其中（F（Q））是我们当前猜测的关节角度的tcp位置。我们希望将误差降至最低，因此，如果我们将δX设置为零，即0 = X–F（Q），我们将得出以下等式:X = F（Q）函数F（）表示我们的正向运动学，正如我们在Rviz教程中简要讨论的那样，它可以很容易地计算出来。不幸的是，我们想根据X知道Q
（相反）这不太容易，所以我们需要做一些技巧。
当我们区分两边时，我们得到一个在机器人学中非常基本的表达式:
v = J（q）∫q
该公式表示tcp速度v
雅可比矩阵J取决于机器人运动学和当前配置中的关节角度q，并将关节速度映射到tcp速度。我们对导致某个tcp姿势的关节角度q感兴趣，因此我们除以雅可比并将其带到等式的另一边:v∫J（q ）- 1 = q
逆J（q ）- 1
只有当雅可比矩阵是方阵时，即自由度的数量与关节的数量n匹配时，才能直接计算雅可比矩阵的。不幸的是，UR5有7个关节，因此我们需要用所谓的伪逆雅可比矩阵（J（q）+）来近似逆雅可比矩阵:v∫J（q）+=ᱸq）
这已经是我们逆向计算需要的主要方程了，让我们看看KDL是怎么做的。正如我们之前已经看到的，当构造ik位置解算器ik_solver时，KDL将计算链ur5_chain、正向运动学解算器fk_solver、IK速度解算器vel_ik_solver和最大迭代次数的整数值作为参数。
之前构建的IK速度解算器vel_ik_solver有三个参数:运动链、误差δX的阈值和最大迭代次数。
KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);
现在让我们看看KDL库中的ik_solver函数，它将笛卡尔tcp位置转换为关节角度。您也可以在这里查看 源文件。
int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
{
if (nj != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
if(q_init.rows() != nj || q_out.rows() != nj)
return (error = E_SIZE_MISMATCH);
q_out = q_init;
unsigned int i;
for(i=0;i<maxiter;i++){
if (E_NOERROR > fksolver.JntToCart(q_out,f) )
return (error = E_FKSOLVERPOS_FAILED);
delta_twist = diff(f,p_in);
const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
if (E_NOERROR > rc)
return (error = E_IKSOLVER_FAILED);
// we chose to continue if the child solver returned a positive
// "error", which may simply indicate a degraded solution
Add(q_out,delta_q,q_out);
if(Equal(delta_twist,Twist::Zero(),eps))
// converged, but possibly with a degraded solution
return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
}
return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge
}
在用初始关节位置q_init初始化关节位置数组q_out之后，用第54行中的正向运动学解算器计算起始tcp位置，这给出了我们的初始F（Q）。在第56行中，初始tcp姿态f和目标tcp姿态p_in之间的距离在1秒的时间范围内进行微分，因此我们得到了tcp速度v（delta _ twist），它同时也是我们的δx .然后我们调用IK速度解算器的CartToJnt（第57行）函数。该函数将当前关节位置估计值q_out（在第一次迭代的情况下为起始关节位置q_init）和速度v作为参数，并使用以下公式计算关节速度q:q = J（q）+∑v该函数将q的结果存储在qdot_out中。
int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
if (nj != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
if (nj != q_in.rows() || nj != qdot_out.rows())
return (error = E_SIZE_MISMATCH);
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
error = jnt2jac.JntToJac(q_in,jac);
if (error < E_NOERROR) return error;
double sum;
unsigned int i,j;
// Initialize near zero singular value counter
nrZeroSigmas = 0 ;
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
svdResult = svd.calculate(jac,U,S,V,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero();
return (error = E_SVD_FAILED);
}
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
//first we calculate Ut*v_in
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
sum+= U[j](i)*v_in(j);
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
if ( fabs(S(i))<eps ) {
tmp(i) = 0.0 ;
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i) ;
}
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=V[i](j)*tmp(j);
}
//Put the result in qdot_out
qdot_out(i)=sum;
}
// Note if the solution is singular, i.e. if number of near zero
// singular values is greater than the full rank of jac
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
在第72行中计算了依赖于关节位置的雅可比矩阵J（q）之后，用第84行中称为奇异值分解的方法来近似伪逆矩阵J（q）+，该方法被称为具有最大迭代量的自变量。分解如下:J（q）+= V÷Spinv÷Ut当计算完成时，得到的q存储在第120行的qdot_out数组中。在返回到IK位置函数后，对关节速度进行积分，因为最终我们想要知道关节位置。通过简单地将q加到关节位置q_out的当前估计值，在第62行的位置ik_solver的CartToJnt函数中完成积分。之后，下一次迭代从这个新值开始。这一直持续到tcp速度v（即δX）位于某个阈值之下。然后，具有估计的关节位置的当前姿态足够接近我们的目的所需的姿态。
就是这样！这就是在KDL进行反向运动学计算的方式。我希望你觉得它有帮助，并能跟随描述。当然，我们没有研究每一个细节，但如果你想知道更多，例如关于奇异值分解，谷歌搜索会给你很多结果。还有其他方法可以实现反向运动学，例如非常流行的ikfast算法，但那是另一个时间。感谢您坚持阅读完整的文章。如果你想学习一种更简单的方法来使用运动规划框架Moveit控制机器人的tcp位置，请查看下一个教程。在这里您可以找到所有教程的概述。
七、ROS Tutorial: How to create a Moveit config for the UR5 and a gripper【Motion Planning】
ROS教程七： 如何为UR5和抓手创建Moveit配置
在本教程中，我们将学习如何使用UR5机械臂和Robotiq手爪创建Moveit配置包，以便在Rviz中进行运动规划。Moveit 是一个非常强大的机器人运动规划框架。在对Moveit进行简要说明后，我们将了解如何为Moveit准备URDF文件并使用Moveit设置助手创建配置包。我还将向您展示需要对生成的包进行哪些额外的修改，以及如何在可视化工具Rviz中使用Moveit插件。
与往常一样， 如果您想快速设置带有抓手的ur5并使用Moveit控制它，而不了解有关如何创建包的详细信息，您可以跳到快速入门部分。否则我们就开始吧！
7.1 Moveit: A motion planning framework
Moveit:一个运动规划框架
什么是运动规划？在之前的教程中，我们看到了在为电机添加一些基本控制器后如何命令机器人到达特定位置。顾名思义，运动规划关注的是机器人如何准确地从起点移动到某个期望的位置。这包括避免途中的障碍和处理约束。有不同的算法来解决这个问题，它们在不同的情况下各有优缺点。
Moveit 是一个将这些算法与您的机器人设置集成在一起的框架。除此之外，它还支持其他方面的操作，如抓握和感知。近年来，它变得非常受欢迎，并围绕它建立了一个伟大的社区（您可以在Moveit网站上找到更多信息）。现在，让我们来看看如何使用Moveit启动和运行我们的机器人手臂和抓手。
7.2 Prepare the URDF files
准备URDF的文件
首先，我们需要一个包含机器人和手爪的URDF。URDF是描述机器人物理属性的文件。要了解更多信息，请查看URDF教程相关教程。
如果您还没有克隆universal_robots库，那么请在您的catkin_ws 工作区中使用以下命令启动您的Linux终端（用您正在使用的ROS发行版替换 < distro>）:
~/hzx2_ws/src$ git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git
此外，将以下存储库克隆到您的catkin_ws 工作空间中，以获取 Robotiq抓取器【地址】 描述:
// 有一次拉取没有拉取完整，有一部分缺失，造成catkin_make时出现错误。
~/hzx2_ws/src$ git clone https://github.com/filesmuggler/robotiq.git
然后下载结合了 ur5机械臂和夹持器/gripper 的URDF文件。在这里，我要对利马UTEC公司的人说声谢谢，感谢他们提供了robotiq软件包和这个URDF文件。切换到universal_robots存储库内的 ur_description/urdf，执行以下命令下载原始文件：若无法成功下载，可以打开网页版，然后新建文件执行复制粘贴【urdf网页地址在此】
~/hzx2_ws/src/universal_robot/ur_description/urdf$ wget https://raw.githubusercontent.com/utecrobotics/ur5/master/ur5_description/urdf/ur5_robotiq85_gripper.urdf.xacro
修改下载的 ur5_robotiq85_gripper.urdf.xacro file文件中的第4行，以便它在我们的 ur_description 包中搜索urdf文件【下载文件中为 ur5_description 】：
// 修改前
<xacro:include filename="$(find ur5_description)/urdf/ur5_joint_limited_robot.urdf.xacro" />
// 修改后
<xacro:include filename="$(find ur_description)/urdf/ur5_joint_limited_robot.urdf.xacro" />
现在我们需要修改机器人和抓手的控制器接口。它们都被配置为 位置关节接口 PositionJointInterface 。我对那个界面有问题，所以我把它改成 力矩关节接口 EffortJointInterface 。以下是修改的内容:
首先， 在robotiq存储库中找到抓取器的传送接口 robotiq_description/urdf/robotiq_85_gripper.transmission.xacro ，我们需要将第9行和第14行改为:
// 文件路径 ~/hzx2_ws/src/robotiq/robotiq_description/urdf/robotiq_85_gripper.transmission.xacro
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
其次， 在universal_robots 存储仓库中的ur_description/urdf/ur5_joint_limited_robot.urdf.xacro中，第5行需要修改如下：该文件下载地址
// ur5_joint_limited_robot.urdf.xacro 文件内容
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro"
name="ur5" >
<xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
<!-- common stuff -->
<xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />
<!-- ur5 -->
<xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
<!-- arm -->
<xacro:ur5_robot prefix="" joint_limited="true"
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="$(arg transmission_hw_interface)"
/>
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
</robot>
<xacro:arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface"/>
最后，我们有了完整的URDF，它描述了带有robotiq抓手的机器人手臂。
7.3 Create a Moveit configuration package with the Moveit Setup Assistant
使用Moveit设置助手创建Moveit配置包
默认情况下应该已经安装了Moveit，所以现在是时候启动Moveit了！使用以下命令的设置助理:
// 任意一个终端下均可运行，但是最好在工作空间路径下，并先执行 source devel/setup.bash ，否则可能会报找不到文件的情况
$ roslaunch moveit_setup_assistant setup_assistant.launch
设置助手将指导我们完成从URDF创建Moveit配置包的必要步骤。在开始屏幕上，我们可以决定是创建新的包还是编辑现有的包。
1、加载编写好的urdf文件，加载成功过后，右边会出现一个机械臂模型。
选择“Create New move it Configuration Package ”,我们可以从文件系统中选择我们的URDF。在universal_robot仓库中选择
/universal_robot/ur_description/urdf/ur5_robotiq85_gripper.urdf.xacro，然后单击“Load Files”。完成后，您应该会看到以下成功消息。
————————————————
若“Load Files”时出现报错：缺失**.xacro，直接安装即可。缺失的.xacro 文件地址在此【是在 kinetic-devel 分支下的/universal_robot/ur_description/urdf/下，缺少什么就拷贝什么】。
2、默认的碰撞免检矩阵生成器搜索机器人所有关节，碰撞检测是检测两个连杆是否有接触，这个碰撞免检矩阵是可以安全地关闭检查，从而减少行动规划的处理时间。比如相邻的两个连杆因为是通过关节连接起来的，但实际上这不是碰撞。所以可以不用去费时间检查。【故xacro文件中的所有link 均应该具有惯性矩阵与碰撞检测部分】
————————————————
转到 “Self-Collisions” 。您可以保持设置不变，然后单击 “Generate Collision Matrix”。当Moveit进行运动规划时，它会检查在运动过程中机器人部件（连杆）之间是否会发生碰撞。这可能需要相当多的计算工作，这就是为什么Moveit试图找到物理上不会相互冲突的link，以将它们从计算中排除。完成后，您应该会看到类似这样的内容:
3、第三步：添加虚拟关节Virtual joint ，添加虚拟关节主要是把机械臂关联到world。这里我们只需要定义一个虚拟关节把base_link关联到world，从而定义机械臂基座和world的坐标系关系。
————————————————
转到 “ Virtual Joints tab ”。定义一个虚拟的固定关节，以便在环境中固定机器人。点击 “Add Virtual Joint”，并填写如下图所示的内容。然后单击 “Save” 。
4、第四步：添加规划组Planning Group
MoveIt 通过定义规划组（planning group）来语义上定义机机械臂的各个部分（如手臂，末端执行器等）。这是MoveIt中一个很重要的概念。简单来说就是定义某些关节为一个组合，并起一个名字。这里我添加了两个规划组，一个是arm，一个是gripper，后面使用moveit提供的python接口会用到，arm控制机械臂运动，gripper控制夹爪运动。
————————————————
接下来，转到 “Planning Groups”。点击 “Add Group” ，让我们先从机械臂开始。填写 “ur5_arm” 作为组名，并选择 kdl _ kinematics _ plugin/kdl kinematics plugin 作为运动学解算器。
然后点击添加 “Add Kin. Chain” 。用左边的小箭头展开框架，直到到达 ee_link。选择 base_link 作为基本链接，ee_link 作为提示链接，然后单击 “Save”。
再次单击 “Add Group” 来定义夹取器。键入 gripper 作为组名，对于运动学解算器，保留 “None” 并单击添加 “Add Joints” 。在左列中选择 “robotiq_85_left_knuckle_joint ” 并单击指向右侧的箭头，以便关节显示在右列中，然后单击 “Save” 。
完成的规划组应该如下所示:
第五步：定义机械臂的一些姿态【初始位姿】
之后继续转到 “Robot Poses”。这一步不是必须的，但是定义基本的位置和姿态可以在将来节省时间。通过设置关节值来创建一个 “home” 姿势。下图显示了ur5的一个有用的原位姿势。这里使用的联合值是:
elbow_joint: 1.5447
shoulder_lift_joint: -1.5447
shoulder_pan_joint: 0.0
wrist_1_joint: -1.5794
wrist_2_joint: -1.5794
wrist_3_joint: 0.0
当你对姿势满意时，你可以点击 “Save”。
让我们也为 夹取器 创建初始姿势。为规划组抓手定义一个新姿势，并将其命名为 “open”。将关节值设置为0.1，然后点击 “Save”；
再创建一个名为 “closed” 的关节，将关节值设置为0.3 并再次点击 “Save”。
第六步：定义末端执行器，这里我用的是gripper组连接到机械臂末端上
为了定义我们的夹取器，切换到 “End Effectors” 选项卡并单击 “Add End Effector”。将其命名为robotiq_gripper，并选择 Choose the Group, Parent Link and Parent Group ，如下图所示。然后单击保存。
第七步：被动关节设置，因为机械臂没有被动关节，所以可以跳过
对于夹取器，我们在下一步要做一些工作，转到 “Passive Joints 。只有一个关节（robotiq_85_left_knuckle_joint）被驱动，因此我们需要通过选择它们并单击箭头将其带到右列来添加所有其他夹爪关节。被动关节包括:
robotiq_85_left_inner_knuckle_joint
robotiq_85_left_finger_tip_joint
robotiq_85_right_inner_knuckle_joint
robotiq_85_right_finger_tip_joint
robotiq_85_right_knuckle_joint
第八步：设置控制器，自动生成arm和gripper的控制器
让我们继续转到 “ROS Contro ”面板。Ros_control是一个允许您轻松设置关节控制器的包（我们之前在本教程中讨论过ros_control包）。在设置助手中，我们只需为每个规划组单击自动添加 FollowJointsTrajctory 控制器。稍后我们将需要修改生成的文件。
我们不需要在"Simulation"窗格中执行任何操作。Moveit可以生成一个URDF，我们可以用它在Gazebo simulation中运行我们的机器人（我们在本教程中讨论了gazebo）。我们已经有这样一份URDF文件。
“3D Preception"窗格用于设置参数，以防我们需要使用3D传感器数据。现在我们一个也不剩。
我们几乎完成了设置助手！
Moveit 需要我们提供一些关于软件包维护者的"Author Information”。您可以键入任何内容，不一定要有效【必填，不然出错，随便写就行】。
最后一步是"Configuration Files"。您可以选择存储配置包的路径。在我们的例子中，我们希望将它存储在我们的catkin_ws 工作空间的源文件夹中。然后点击生成包和移动它会做它的职责！
在使用Moveit包之前，我们还需要做一些小的修改。
7.4 Modifications to the Moveit configuration package
对Moveit配置包的修改
如果你查看 ur5_gripper_moveit_config 包文件夹，你会发现那里有很多生成的文件。我们首先要修改ros_controllers.yaml。该文件包含我们用于机器人关节的控制器的描述。让我们打开位于config子文件夹中的文件。有一个ROS会抱怨的错误，要解决它，我们只需在第42行的 robotiq_85_left_knuckle_joint 前面添加一个破折号。我们还需要添加用于Gazebo中模拟的控制器。您可以在文件末尾添加以下代码片段。或者您可以在 此链接 下找到完全修改过的ros_controllers.yaml文件。
ur5_arm_controller:
type: effort_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
gains:
shoulder_pan_joint:
p: 1000
d: 50
i: 10
i_clamp: 100
shoulder_lift_joint:
p: 5000
d: 30
i: 100
i_clamp: 400
elbow_joint:
p: 5000
d: 50
i: 10
i_clamp: 100
wrist_1_joint:
p: 200
d: 20
i: 10
i_clamp: 100
wrist_2_joint:
p: 100
d: 10
i: 0.1
i_clamp: 100
wrist_3_joint:
p: 10
d: 1
i: 0.1
i_clamp: 100
gripper_controller:
type: effort_controllers/JointTrajectoryController
joints:
- robotiq_85_left_knuckle_joint
gains:
robotiq_85_left_knuckle_joint:
p: 40
d: 0
i: 0
i_clamp: 1
我们使用的控制器参数并不完美，但它们发挥了作用。我们需要修改的第二个文件是launch子文件夹中的 ros_controllers.launch。 在第9行中，添加我们希望作为参数加载到控制器生成器的控制器:
output="screen" args="ur5_arm_controller gripper_controller joint_state_controller"
最后，我们必须在launch文件夹中的gazebo.launch中为机器人的产卵位置添加一个偏移量，您可能从gazebo教程中了解到这一点。我们希望在其中生成机器人的空世界有一个隐形的底板，它会导致与机器人发生碰撞。这可能会导致Moveit抱怨一些错误信息，因为机器人在抖动。为了避免这种情况，将偏移量添加到第18行spawn_gazebo_model节点的参数中，如下所示:
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0.1"
就是这样！现在Moveit包可以使用了。如果由于某种原因您没有成功创建包，您可以用已经进行了必要修改的包克隆我的git存储库:
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_gripper_moveit_config
7.5 Use Moveit for motion planning in Rviz
在Rviz中使用Moveit进行运动规划
要启动Rviz和Gazebo，请在您的终端中执行以下命令:
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch
在这两个图中，您现在应该看到UR5机器人连接了一个手爪。现在我们可以在Rviz中尝试运动规划。如果在Rviz的左下角没有看到 MotionPlanning 部分，点击左下角的 Add，在display_type下选择 MotionPlanning。
在“Planning”标签中，您可以选择"the Planning group"。当您选择ur5_arm并选择左上角的“交互”时【Interact in the top left corner】，您应该会看到出现在机器人手臂尖端的箭头。这些可以让你和机器人互动，改变它的位置。拖动机器人，然后点击“Plan”和“the motion planner”规划如何移动到所需的位置。计算出的计划将显示在Rviz中。现在你可以点击“Execute”，机器人就会真正地移动。
由于控制器中的一些偏差，Moveit 可能会显示失败而不是成功的消息。这是因为我们没有完美调整的控制参数。然而，机器人手臂仍然以足够高的精度移动到所需位置。
当我们选择抓手规划组时，我们没有箭头与之交互。尽管如此，如果我们切换到“the joints tab”（Moveit插件中最右边的选项卡），我们可以直接设置抓取关节的关节值。然后，我们可以返回到“Planning tab”选项卡，以与机器人手臂相同的方式进行“ and plan and execute”。
在Rviz内部使用Moveit就是这么简单！该插件提供了更多的功能，我们现在没有时间讨论。Moveit 是一个广泛而强大的工具。在下一篇教程中，我将展示如何使用Moveit C++接口实现 拾取和放置 任务。在那里，我们将使用我们刚刚创建的Moveit配置。也看看其他教程，我很高兴在评论中收到一些反馈。
八、ROS Tutorial: Pick and Place task with the Moveit C++ interface【Motion Planning】
ROS教程八： 使用Moveit C++接口拾取和放置任务
本教程展示了如何使用Moveit C++接口实现一个简单的拾取和放置任务。在上一期教程中，我向您展示了如何为UR5机械臂和Robotiq手爪创建Moveit配置，以及如何使用Rviz中的Moveit插件对其进行控制。现在我们可以使用这种配置并使用C++代码命令我们的机器人。
在简单介绍了界面之后，我将展示我们用于拾取和放置任务的“gazebo world”。之后我们将一步一步地检查代码。最后还将学习如何通过在规划场景中添加碰撞对象来避免碰撞。
8.1 The Moveit C++ Interface
Moveit C++接口
让我们简单谈谈C++接口以及它如何连接到Moveit的其余部分。正如官方网站上的这张图片所示，Moveit具有不同的接口，我们可以使用这些接口来实现我们的应用程序:
来自Moveit官方网站的系统架构概述
Rviz中的GUI界面我们已经从前面的教程中了解过了。对于编码，Moveit为Python提供了moveit_commander接口，为C++提供了move_group_interface接口。该图还显示了与中心move_group节点之间的其他连接。在上一个教程中创建Moveit配置时，我们使用URDF文件创建SRDF，并使用Moveit创建配置！设置助理。Move_group从ROS参数服务器获取所有这些信息。
运动命令作为轨迹发送给机器人的控制器（例如用ros_control设置的控制器）。Moveit C++接口提供了一种访问move_group节点最重要功能的简单方法，让我们开始吧！
8.2 The Gazebo Pick and Place World
Gazebo取放世界
在上一个教程中，您可能已经有了带有 Robotiq夹持器的UR5的Moveit配置包 。如果没有，请遵循上一篇文章的快速入门部分。
为了实现拾取和放置任务，我们创建了一个新的ROS包。
您可以克隆完成的包
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_simple_pick_and_place
或者您可以继续创建自己的工作包。
在包内我创建了一个新的Gazebo world，如下所示：【给了机械手臂一个初始位姿】
为了加载这个世界，我们需要修改Moveit配置包中的gazebo.launch文件。因此，如果您已经开始了gazebo演示，那么请关闭它，继续更改第9行，以默认方式加载拾取和放置世界:
<arg name="world_name" default="$(find ur5_simple_pick_and_place)/world/simple_pick_and_place"/>
在这个新世界中，机器人被放在桌子上，因此我们需要修改机器人手臂生成的位置。因此，我们需要将第18行中z轴的偏移量更改为桌子的高度（大约1.21米）:
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 1.21"
现在您可以开始gazebo演示了：【载入手臂模型与搭建的场景】
// 在工作空间 路径~/catkin_ws$ ，执行：
roslaunch ur5_gripper_moveit_config demo_gazebo.launch
// 正确启动launch后的日志输出：
hzx@vm:~/Music/demo088_ws$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch
... logging to /home/hzx/.ros/log/f651d6b8-118a-11ef-9db1-000c29915674/roslaunch-vm-5468.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
Error in sitecustomize; set PYTHONVERBOSE for traceback:
NameError: name 'reload' is not defined
Error in sitecustomize; set PYTHONVERBOSE for traceback:
NameError: name 'reload' is not defined
started roslaunch server http://vm:45841/
SUMMARY
========
PARAMETERS
* /controller_list: [{'default': True...
* /gazebo/enable_ros_network: True
* /generic_hw_control_loop/cycle_time_error_threshold: 0.01
* /generic_hw_control_loop/loop_hz: 300
* /gripper_controller/gains/robotiq_85_left_knuckle_joint/d: 0
* /gripper_controller/gains/robotiq_85_left_knuckle_joint/i: 0
* /gripper_controller/gains/robotiq_85_left_knuckle_joint/i_clamp: 1
* /gripper_controller/gains/robotiq_85_left_knuckle_joint/p: 40
* /gripper_controller/joints: ['robotiq_85_left...
* /gripper_controller/type: effort_controller...
* /hardware_interface/joints: ['shoulder_pan_jo...
* /hardware_interface/sim_control_mode: 1
* /joint_state_controller/publish_rate: 50
* /joint_state_controller/type: joint_state_contr...
* /joint_state_publisher/source_list: ['/joint_states']
* /joint_state_publisher/use_gui: False
* /move_group/allow_trajectory_execution: True
* /move_group/capabilities:
* /move_group/controller_list: [{'default': True...
* /move_group/disable_capabilities:
* /move_group/generic_hw_control_loop/cycle_time_error_threshold: 0.01
* /move_group/generic_hw_control_loop/loop_hz: 300
* /move_group/gripper/default_planner_config:
* /move_group/gripper/planner_configs: ['SBL', 'EST', 'L...
* /move_group/gripper_controller/gains/robotiq_85_left_knuckle_joint/d: 0
* /move_group/gripper_controller/gains/robotiq_85_left_knuckle_joint/i: 0
* /move_group/gripper_controller/gains/robotiq_85_left_knuckle_joint/i_clamp: 1
* /move_group/gripper_controller/gains/robotiq_85_left_knuckle_joint/p: 40
* /move_group/gripper_controller/joints: ['robotiq_85_left...
* /move_group/gripper_controller/type: effort_controller...
* /move_group/hardware_interface/joints: ['shoulder_pan_jo...
* /move_group/hardware_interface/sim_control_mode: 1
* /move_group/jiggle_fraction: 0.05
* /move_group/joint_state_controller/publish_rate: 50
* /move_group/joint_state_controller/type: joint_state_contr...
* /move_group/max_range: 5.0
* /move_group/max_safe_path_cost: 1
* /move_group/moveit_controller_manager: moveit_simple_con...
* /move_group/moveit_manage_controllers: True
* /move_group/moveit_sim_hw_interface/joint_model_group: ur5_arm
* /move_group/moveit_sim_hw_interface/joint_model_group_pose: home
* /move_group/octomap_frame: base_frame
* /move_group/octomap_resolution: 0.025
* /move_group/planner_configs/BFMT/balanced: 0
* /move_group/planner_configs/BFMT/cache_cc: 1
* /move_group/planner_configs/BFMT/extended_fmt: 1
* /move_group/planner_configs/BFMT/heuristics: 1
* /move_group/planner_configs/BFMT/nearest_k: 1
* /move_group/planner_configs/BFMT/num_samples: 1000
* /move_group/planner_configs/BFMT/optimality: 1
* /move_group/planner_configs/BFMT/radius_multiplier: 1.0
* /move_group/planner_configs/BFMT/type: geometric::BFMT
* /move_group/planner_configs/BKPIECE/border_fraction: 0.9
* /move_group/planner_configs/BKPIECE/failed_expansion_score_factor: 0.5
* /move_group/planner_configs/BKPIECE/min_valid_path_fraction: 0.5
* /move_group/planner_configs/BKPIECE/range: 0.0
* /move_group/planner_configs/BKPIECE/type: geometric::BKPIECE
* /move_group/planner_configs/BiEST/range: 0.0
* /move_group/planner_configs/BiEST/type: geometric::BiEST
* /move_group/planner_configs/BiTRRT/cost_threshold: 1e300
* /move_group/planner_configs/BiTRRT/frountier_node_ratio: 0.1
* /move_group/planner_configs/BiTRRT/frountier_threshold: 0.0
* /move_group/planner_configs/BiTRRT/init_temperature: 100
* /move_group/planner_configs/BiTRRT/range: 0.0
* /move_group/planner_configs/BiTRRT/temp_change_factor: 0.1
* /move_group/planner_configs/BiTRRT/type: geometric::BiTRRT
* /move_group/planner_configs/EST/goal_bias: 0.05
* /move_group/planner_configs/EST/range: 0.0
* /move_group/planner_configs/EST/type: geometric::EST
* /move_group/planner_configs/FMT/cache_cc: 1
* /move_group/planner_configs/FMT/extended_fmt: 1
* /move_group/planner_configs/FMT/heuristics: 0
* /move_group/planner_configs/FMT/nearest_k: 1
* /move_group/planner_configs/FMT/num_samples: 1000
* /move_group/planner_configs/FMT/radius_multiplier: 1.1
* /move_group/planner_configs/FMT/type: geometric::FMT
* /move_group/planner_configs/KPIECE/border_fraction: 0.9
* /move_group/planner_configs/KPIECE/failed_expansion_score_factor: 0.5
* /move_group/planner_configs/KPIECE/goal_bias: 0.05
* /move_group/planner_configs/KPIECE/min_valid_path_fraction: 0.5
* /move_group/planner_configs/KPIECE/range: 0.0
* /move_group/planner_configs/KPIECE/type: geometric::KPIECE
* /move_group/planner_configs/LBKPIECE/border_fraction: 0.9
* /move_group/planner_configs/LBKPIECE/min_valid_path_fraction: 0.5
* /move_group/planner_configs/LBKPIECE/range: 0.0
* /move_group/planner_configs/LBKPIECE/type: geometric::LBKPIECE
* /move_group/planner_configs/LBTRRT/epsilon: 0.4
* /move_group/planner_configs/LBTRRT/goal_bias: 0.05
* /move_group/planner_configs/LBTRRT/range: 0.0
* /move_group/planner_configs/LBTRRT/type: geometric::LBTRRT
* /move_group/planner_configs/LazyPRM/range: 0.0
* /move_group/planner_configs/LazyPRM/type: geometric::LazyPRM
* /move_group/planner_configs/LazyPRMstar/type: geometric::LazyPR...
* /move_group/planner_configs/PDST/type: geometric::PDST
* /move_group/planner_configs/PRM/max_nearest_neighbors: 10
* /move_group/planner_configs/PRM/type: geometric::PRM
* /move_group/planner_configs/PRMstar/type: geometric::PRMstar
* /move_group/planner_configs/ProjEST/goal_bias: 0.05
* /move_group/planner_configs/ProjEST/range: 0.0
* /move_group/planner_configs/ProjEST/type: geometric::ProjEST
* /move_group/planner_configs/RRT/goal_bias: 0.05
* /move_group/planner_configs/RRT/range: 0.0
* /move_group/planner_configs/RRT/type: geometric::RRT
* /move_group/planner_configs/RRTConnect/range: 0.0
* /move_group/planner_configs/RRTConnect/type: geometric::RRTCon...
* /move_group/planner_configs/RRTstar/delay_collision_checking: 1
* /move_group/planner_configs/RRTstar/goal_bias: 0.05
* /move_group/planner_configs/RRTstar/range: 0.0
* /move_group/planner_configs/RRTstar/type: geometric::RRTstar
* /move_group/planner_configs/SBL/range: 0.0
* /move_group/planner_configs/SBL/type: geometric::SBL
* /move_group/planner_configs/SPARS/dense_delta_fraction: 0.001
* /move_group/planner_configs/SPARS/max_failures: 1000
* /move_group/planner_configs/SPARS/sparse_delta_fraction: 0.25
* /move_group/planner_configs/SPARS/stretch_factor: 3.0
* /move_group/planner_configs/SPARS/type: geometric::SPARS
* /move_group/planner_configs/SPARStwo/dense_delta_fraction: 0.001
* /move_group/planner_configs/SPARStwo/max_failures: 5000
* /move_group/planner_configs/SPARStwo/sparse_delta_fraction: 0.25
* /move_group/planner_configs/SPARStwo/stretch_factor: 3.0
* /move_group/planner_configs/SPARStwo/type: geometric::SPARStwo
* /move_group/planner_configs/STRIDE/degree: 16
* /move_group/planner_configs/STRIDE/estimated_dimension: 0.0
* /move_group/planner_configs/STRIDE/goal_bias: 0.05
* /move_group/planner_configs/STRIDE/max_degree: 18
* /move_group/planner_configs/STRIDE/max_pts_per_leaf: 6
* /move_group/planner_configs/STRIDE/min_degree: 12
* /move_group/planner_configs/STRIDE/min_valid_path_fraction: 0.2
* /move_group/planner_configs/STRIDE/range: 0.0
* /move_group/planner_configs/STRIDE/type: geometric::STRIDE
* /move_group/planner_configs/STRIDE/use_projected_distance: 0
* /move_group/planner_configs/TRRT/frountierNodeRatio: 0.1
* /move_group/planner_configs/TRRT/frountier_threshold: 0.0
* /move_group/planner_configs/TRRT/goal_bias: 0.05
* /move_group/planner_configs/TRRT/init_temperature: 10e-6
* /move_group/planner_configs/TRRT/k_constant: 0.0
* /move_group/planner_configs/TRRT/max_states_failed: 10
* /move_group/planner_configs/TRRT/min_temperature: 10e-10
* /move_group/planner_configs/TRRT/range: 0.0
* /move_group/planner_configs/TRRT/temp_change_factor: 2.0
* /move_group/planner_configs/TRRT/type: geometric::TRRT
* /move_group/planning_plugin: ompl_interface/OM...
* /move_group/planning_scene_monitor/publish_geometry_updates: True
* /move_group/planning_scene_monitor/publish_planning_scene: True
* /move_group/planning_scene_monitor/publish_state_updates: True
* /move_group/planning_scene_monitor/publish_transforms_updates: True
* /move_group/request_adapters: default_planner_r...
* /move_group/sensors: [{'point_subsampl...
* /move_group/start_state_max_bounds_error: 0.1
* /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
* /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
* /move_group/trajectory_execution/allowed_start_tolerance: 0.01
* /move_group/ur5_arm/default_planner_config: SBL
* /move_group/ur5_arm/longest_valid_segment_fraction: 0.005
* /move_group/ur5_arm/planner_configs: ['SBL', 'EST', 'L...
* /move_group/ur5_arm/projection_evaluator: joints(shoulder_p...
* /move_group/ur5_arm_controller/gains/elbow_joint/d: 50
* /move_group/ur5_arm_controller/gains/elbow_joint/i: 10
* /move_group/ur5_arm_controller/gains/elbow_joint/i_clamp: 100
* /move_group/ur5_arm_controller/gains/elbow_joint/p: 5000
* /move_group/ur5_arm_controller/gains/shoulder_lift_joint/d: 30
* /move_group/ur5_arm_controller/gains/shoulder_lift_joint/i: 100
* /move_group/ur5_arm_controller/gains/shoulder_lift_joint/i_clamp: 400
* /move_group/ur5_arm_controller/gains/shoulder_lift_joint/p: 5000
* /move_group/ur5_arm_controller/gains/shoulder_pan_joint/d: 50
* /move_group/ur5_arm_controller/gains/shoulder_pan_joint/i: 10
* /move_group/ur5_arm_controller/gains/shoulder_pan_joint/i_clamp: 100
* /move_group/ur5_arm_controller/gains/shoulder_pan_joint/p: 1000
* /move_group/ur5_arm_controller/gains/wrist_1_joint/d: 20
* /move_group/ur5_arm_controller/gains/wrist_1_joint/i: 10
* /move_group/ur5_arm_controller/gains/wrist_1_joint/i_clamp: 100
* /move_group/ur5_arm_controller/gains/wrist_1_joint/p: 200
* /move_group/ur5_arm_controller/gains/wrist_2_joint/d: 10
* /move_group/ur5_arm_controller/gains/wrist_2_joint/i: 0.1
* /move_group/ur5_arm_controller/gains/wrist_2_joint/i_clamp: 100
* /move_group/ur5_arm_controller/gains/wrist_2_joint/p: 100
* /move_group/ur5_arm_controller/gains/wrist_3_joint/d: 1
* /move_group/ur5_arm_controller/gains/wrist_3_joint/i: 0.1
* /move_group/ur5_arm_controller/gains/wrist_3_joint/i_clamp: 100
* /move_group/ur5_arm_controller/gains/wrist_3_joint/p: 10
* /move_group/ur5_arm_controller/joints: ['shoulder_pan_jo...
* /move_group/ur5_arm_controller/type: effort_controller...
* /moveit_sim_hw_interface/joint_model_group: ur5_arm
* /moveit_sim_hw_interface/joint_model_group_pose: home
* /robot_description: <?xml version="1....
* /robot_description_kinematics/ur5_arm/kinematics_solver: kdl_kinematics_pl...
* /robot_description_kinematics/ur5_arm/kinematics_solver_search_resolution: 0.005
* /robot_description_kinematics/ur5_arm/kinematics_solver_timeout: 0.005
* /robot_description_planning/joint_limits/elbow_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/elbow_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/elbow_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/elbow_joint/max_velocity: 3.15
* /robot_description_planning/joint_limits/robotiq_85_left_knuckle_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/robotiq_85_left_knuckle_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/robotiq_85_left_knuckle_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/robotiq_85_left_knuckle_joint/max_velocity: 0.5
* /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 3.15
* /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 3.15
* /robot_description_planning/joint_limits/wrist_1_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/wrist_1_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_1_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/wrist_1_joint/max_velocity: 3.2
* /robot_description_planning/joint_limits/wrist_2_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/wrist_2_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_2_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/wrist_2_joint/max_velocity: 3.2
* /robot_description_planning/joint_limits/wrist_3_joint/has_acceleration_limits: False
* /robot_description_planning/joint_limits/wrist_3_joint/has_velocity_limits: True
* /robot_description_planning/joint_limits/wrist_3_joint/max_acceleration: 0
* /robot_description_planning/joint_limits/wrist_3_joint/max_velocity: 3.2
* /robot_description_semantic: <?xml version="1....
* /rosdistro: melodic
* /rosversion: 1.14.13
* /ur5_arm_controller/gains/elbow_joint/d: 50
* /ur5_arm_controller/gains/elbow_joint/i: 10
* /ur5_arm_controller/gains/elbow_joint/i_clamp: 100
* /ur5_arm_controller/gains/elbow_joint/p: 5000
* /ur5_arm_controller/gains/shoulder_lift_joint/d: 30
* /ur5_arm_controller/gains/shoulder_lift_joint/i: 100
* /ur5_arm_controller/gains/shoulder_lift_joint/i_clamp: 400
* /ur5_arm_controller/gains/shoulder_lift_joint/p: 5000
* /ur5_arm_controller/gains/shoulder_pan_joint/d: 50
* /ur5_arm_controller/gains/shoulder_pan_joint/i: 10
* /ur5_arm_controller/gains/shoulder_pan_joint/i_clamp: 100
* /ur5_arm_controller/gains/shoulder_pan_joint/p: 1000
* /ur5_arm_controller/gains/wrist_1_joint/d: 20
* /ur5_arm_controller/gains/wrist_1_joint/i: 10
* /ur5_arm_controller/gains/wrist_1_joint/i_clamp: 100
* /ur5_arm_controller/gains/wrist_1_joint/p: 200
* /ur5_arm_controller/gains/wrist_2_joint/d: 10
* /ur5_arm_controller/gains/wrist_2_joint/i: 0.1
* /ur5_arm_controller/gains/wrist_2_joint/i_clamp: 100
* /ur5_arm_controller/gains/wrist_2_joint/p: 100
* /ur5_arm_controller/gains/wrist_3_joint/d: 1
* /ur5_arm_controller/gains/wrist_3_joint/i: 0.1
* /ur5_arm_controller/gains/wrist_3_joint/i_clamp: 100
* /ur5_arm_controller/gains/wrist_3_joint/p: 10
* /ur5_arm_controller/joints: ['shoulder_pan_jo...
* /ur5_arm_controller/type: effort_controller...
* /use_sim_time: True
NODES
/
controller_spawner (controller_manager/spawner)
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
joint_state_publisher (joint_state_publisher/joint_state_publisher)
move_group (moveit_ros_move_group/move_group)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz_vm_5468_5905208729026277587 (rviz/rviz)
spawn_gazebo_model (gazebo_ros/spawn_model)
auto-starting new master
process[master]: started with pid [5485]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to f651d6b8-118a-11ef-9db1-000c29915674
process[rosout-1]: started with pid [5504]
started core service [/rosout]
process[gazebo-2]: started with pid [5517]
process[gazebo_gui-3]: started with pid [5522]
process[spawn_gazebo_model-4]: started with pid [5527]
process[controller_spawner-5]: started with pid [5528]
process[joint_state_publisher-6]: started with pid [5529]
process[robot_state_publisher-7]: started with pid [5530]
process[move_group-8]: started with pid [5531]
process[rviz_vm_5468_5905208729026277587-9]: started with pid [5536]
[ INFO] [1715647404.082631820]: Loading robot model 'ur5'...
[ INFO] [1715647404.857160287]: rviz version 1.13.30
[ INFO] [1715647404.857596185]: compiled against Qt version 5.9.5
[ INFO] [1715647404.857743344]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1715647404.898812605]: Forcing OpenGl version 0.
[INFO] [1715647405.586348, 0.000000]: Controller Spawner: Waiting for service controller_manager/load_controller
[ INFO] [1715647405.852985051]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1715647405.858978716]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1715647406.007410485]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1715647406.016635741]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1715647406.287466103]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1715647406.294589535]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1715647406.294623025]: Starting planning scene monitor
[ INFO] [1715647406.302137759]: Listening to '/planning_scene'
[ INFO] [1715647406.302310340]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1715647406.307809837]: Listening to '/collision_object'
[ INFO] [1715647406.314151161]: Listening to '/planning_scene_world' for planning scene world geometry
[INFO] [1715647406.873816, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1715647406.902801, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1715647407.282322373]: Listening to '/camera1/depth/points' using message filter with target frame 'world '
[ INFO] [1715647407.294724126]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1715647407.341530884]: Initializing OMPL interface using ROS parameters
[ INFO] [1715647407.426656373]: Using planning interface 'OMPL'
[ INFO] [1715647407.434458164]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1715647407.435943689]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1715647407.436626281]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1715647407.438678366]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1715647407.439633514]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1715647407.440615246]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1715647407.440770463]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1715647407.440870807]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1715647407.440931765]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1715647407.440992985]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1715647407.441052853]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1715647407.627388638]: Stereo is NOT SUPPORTED
[ INFO] [1715647407.627557060]: OpenGL device: SVGA3D; build: RELEASE; LLVM;
[ INFO] [1715647407.627616911]: OpenGl version: 2.1 (GLSL 1.2).
[ INFO] [1715647409.001916532]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [1715647409.039361, 0.000000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1715647409.865144, 0.001000]: Spawn status: SpawnModel: Successfully spawned entity
[INFO] [1715647409.879883, 0.001000]: Waiting for service /gazebo/set_model_configuration
[INFO] [1715647409.907540, 0.001000]: temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.
[ INFO] [1715647409.921274184, 0.001000000]: Physics dynamic reconfigure ready.
[ INFO] [1715647410.268859272, 0.001000000]: Loading gazebo_ros_control plugin
[ INFO] [1715647410.269171192, 0.001000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1715647410.271400904, 0.001000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ INFO] [1715647410.419068484, 0.001000000]: Loaded gazebo_ros_control.
[INFO] [1715647410.435794, 0.001000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1715647410.443037, 0.001000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1715647410.452194, 0.001000]: Loading controller: ur5_arm_controller
[ INFO] [1715647410.463895758, 0.001000000]: ft_sensor plugin reporting wrench values to the frame [wrist_3_link]
[INFO] [1715647410.912819, 0.286000]: Calling service /gazebo/set_model_configuration
[INFO] [1715647410.922958, 0.291000]: Set model configuration status: SetModelConfiguration: success
[INFO] [1715647411.109568, 0.412000]: Loading controller: gripper_controller
[spawn_gazebo_model-4] process has finished cleanly
log file: /home/hzx/.ros/log/f651d6b8-118a-11ef-9db1-000c29915674/spawn_gazebo_model-4*.log
[INFO] [1715647411.249209, 0.495000]: Loading controller: joint_state_controller
[INFO] [1715647411.276934, 0.509000]: Controller Spawner: Loaded controllers: ur5_arm_controller, gripper_controller, joint_state_controller
[INFO] [1715647411.294575, 0.519000]: Started controllers: ur5_arm_controller, gripper_controller, joint_state_controller
[ INFO] [1715647411.449319265, 0.602000000]: Added FollowJointTrajectory controller for ur5_arm_controller
[ INFO] [1715647411.770487363, 0.790000000]: Added FollowJointTrajectory controller for gripper_controller
[ INFO] [1715647411.770698823, 0.790000000]: Returned 2 controllers in list
[ INFO] [1715647411.801981941, 0.800000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1715647411.950212819, 0.873000000]:
********************************************************
* MoveGroup using:
* - ApplyPlanningSceneService
* - ClearOctomapService
* - CartesianPathService
* - ExecuteTrajectoryAction
* - GetPlanningSceneService
* - KinematicsService
* - MoveAction
* - PickPlaceAction
* - MotionPlanService
* - QueryPlannersService
* - StateValidationService
********************************************************
[ INFO] [1715647411.950539736, 0.874000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1715647411.950573273, 0.874000000]: MoveGroup context initialization complete
You can start planning now!
[ INFO] [1715647415.642699766, 3.348000000]: Loading robot model 'ur5'...
[ INFO] [1715647417.243461110, 4.225000000]: Starting planning scene monitor
[ INFO] [1715647417.249819364, 4.229000000]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1715647418.517574998, 5.123000000]: No active joints or end effectors found for group 'gripper'. Make sure that kinematics.yaml is loaded in this node's namespace.
[ INFO] [1715647418.521162992, 5.130000000]: No active joints or end effectors found for group 'gripper'. Make sure that kinematics.yaml is loaded in this node's namespace.
[ INFO] [1715647418.535869081, 5.139000000]: Constructing new MoveGroup connection for group 'gripper' in namespace ''
[ INFO] [1715647419.595615517, 5.864000000]: Ready to take commands for planning group gripper.
[ INFO] [1715647419.595959701, 5.864000000]: Looking around: no
[ INFO] [1715647419.596075981, 5.864000000]: Replanning: no
^C[rviz_vm_5468_5905208729026277587-9] killing on exit
[move_group-8] killing on exit
[robot_state_publisher-7] killing on exit
[controller_spawner-5] killing on exit
[INFO] [1715648129.211577, 474.005000]: Shutting down spawner. Stopping and unloading controllers...
[INFO] [1715648129.214773, 474.005000]: Stopping all controllers...
[gazebo_gui-3] killing on exit
[joint_state_publisher-6] killing on exit
[gazebo-2] killing on exit
^Z
[3]+ 已停止 roslaunch ur5_gripper_moveit_config demo_gazebo.launch
hzx@vm:~/Music/demo088_ws$
解决一些报错：
1、Skipping virtual joint ‘virtual_joint’ because its child frame ‘base_link’ does not match the URDF frame ‘world’
解决方案：
找到该文件 catkin_ws/src/ur5_gripper_moveit_config/config/ur5.srdf
修改如下内容：
由：
<virtual_joint name=“virtual_joint” type=“fixed” parent_frame=“world” child_link=“base_link” />
修改为：
<virtual_joint name=“virtual_joint” type=“fixed” parent_frame=“world” child_link=“world” />
2、Spawn service failed. Exiting.
解决方案：
找到该文件 catkin_ws/src/ur5_simple_pick_and_place/world/simple_pick_and_place
修改如下内容：
由：
<sim_time>2412 519000000</sim_time>
修改为：
<sim_time>0</sim_time>
3、Could not find the planner configuration ‘None’ on the param server
解决方案：
找到该文件 catkin_ws/src/ur5_gripper_moveit_config/config/ompl_planning.yaml
修改如下内容：
由：
default_planner_config: None
修改为：
default_planner_config: SBL
4、SpawnModel: Failure - model name robot already exist.
该文章对此解释的很清楚，具体解决方案也可参考此做法
5、启动launch文件后，gazebo 场景中不显示“nist_elevated_floor_120【桌子】”或者“bowl【白盘子】”等部件
该文章对此解释的很清楚，具体解决方案也可参考此做法
/opt/ros/melodic/share/gazebo_ros/launch/empty_world.launch
6、process has died [pid 397, exit code 255, cmd /opt/ros/melodic/lib/gazebo_ros/gzserver -e ode worlds/empty.world
该文章对此解释的很清楚，具体解决方案也可参考此做法
也可以执行以下指令：
killall gzserver && killall gzclient
8.3 Initializing the interface
初始化接口
我们的任务是把蓝色的盒子放在右手边的盘子里。我们的C++代码位于拾取和放置库的src文件夹中一个名为simple_pick_and_place.cpp的文件中（您可以在此链接下查看完整的文件）。在我们能够控制我们的动作之前，我们需要做一些初始化。
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle n;
// ROS spinning must be running for the MoveGroupInterface to get information
// about the robot's state. One way to do this is to start an AsyncSpinner
// beforehand.
ros::AsyncSpinner spinner(1);
spinner.start();
// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP_ARM = "ur5_arm";
static const std::string PLANNING_GROUP_GRIPPER = "gripper";
// The :planning_interface:`MoveGroupInterface` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group_interface_arm(PLANNING_GROUP_ARM);
moveit::planning_interface::MoveGroupInterface move_group_interface_gripper(PLANNING_GROUP_GRIPPER);
像往常一样，当创建一个ROS节点时，我们有一个main函数。在第7行我们初始化了ROS，在第8行我们创建了NodeHandle。接下来，我们在第13行和第14行实例化并启动一个异步旋转器。这一点很重要，因为spinner确保主题不断地被发送和接收。move_group需要它来更新机器人状态。
我们需要两个move_group接口，一个用于机械臂，一个用于抓爪，因为它们是独立的规划组。在第19行和第20行，我们将规划组的名称定义为“ur5_arm”和“gripper”。在第24和25行中，我们使用这些名称来实例化MoveGroupInterfaces。
8.4 Commanding the motions
控制运动
我们将取放任务分成几个动作步骤:
1、Move to home position 移动到初始位置
2、Place the TCP (Tool Center Point, the tip of the robot) above the blue box 将TCP（工具中心点，机器人的尖端）放置在蓝色框上方
3、Open the gripper 打开夹子
4、Move the TCP close to the object 将TCP移近对象
5、Close the gripper 关闭夹子
6、Move the TCP above the plate 将TCP移到板上方
7、Lower the TCP above the plate 将TCP下放至板上方
8、Open the gripper 打开夹子
最终的运动如下所示:
开始移动之前，我们可以打印可用的计划组（第29/30行）。这样我们就知道这些组是否被正确加载。
// We can get a list of all the groups in the robot:
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group_interface_arm.getJointModelGroupNames().begin(),
move_group_interface_arm.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
1.移动到初始位置
在第32行中，我们实例化了一个moveit::planning _ interface::movegroup interface::Plan，我们称之为my_plan_arm。我们稍后将使用它来存储运动规划器创建的运动计划。为UR5机械臂设置目标位置有不同的选项。一种方法是定义机器人关节的目标位置，另一种方法是定义TCP的目标姿态。
moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
// 1. Move to home position
move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("home"));
bool success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
在第一步中，我们希望移动到在创建Moveit配置包时定义的“home”位置。此初始位置被定义为srdf文件中的一组关节位置值。我们可以通过调用move _ group _ interface _ arm . getnamedtargetvalues（“home“）来获取这些值。然后我们通过调用setJointValueTarget将这些值传递给move_group，这发生在第35行。
使用第37行中的move _ group _ interface _ arm . plan（my_plan_arm），我们调用运动规划器，将其结果存储在我们之前创建的my _ plan _ arm中。我们还检查计划是否成功创建，并在第39行打印结果。到目前为止，我们只有一个运动规划者的计划，但机器人还没有移动。为了实际执行移动，我们需要调用move_group_interface_arm.move（）（第41行）。现在，我们的拾取和放置序列的第一步已经完成
2.将TCP放在蓝色盒子上方
在第二步中，我们移动到蓝框上方的位置。我知道位置，因为我创建了gazebo世界，但您可以通过单击对象并检查右侧显示的位置来进行检查。在这一步中，我们希望定义一个目标姿势，而不是关节值，因此我们在第47行中对名为target_pose1的geometry_msgs::Pose进行insantiate。姿态由x、y和z位置以及TCP的方向组成。我们希望保持方向不变，因此我们在第44行和第45行中获取末端效应器链接的当前姿势，并在第49行中将当前方向设置为目标方向。
// 2. Place the TCP (Tool Center Point, the tip of the robot) above the blue box
geometry_msgs::PoseStamped current_pose;
current_pose = move_group_interface_arm.getCurrentPose("ee_link");
geometry_msgs::Pose target_pose1;
target_pose1.orientation = current_pose.pose.orientation;
target_pose1.position.x = 0.3;
target_pose1.position.y = 0.5;
target_pose1.position.z = 0.2;
move_group_interface_arm.setPoseTarget(target_pose1);
success = (move_group_interface_arm.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group_interface_arm.move();
对于坐标轴，我选择了相同的坐标，除了z位置（第50行到第52行）。这一个有点棘手，因为目标姿势的参考帧是我们机器人的基础帧。对于这个帧，我们已经有一个从世界帧的偏移，因为机器人是在z位置1.21产生的。反过来，框在gazebo中显示的位置是相对于世界坐标系显示的。尽管如此，我们可以假设盒子和机器人底座在大约相同的z位置，因为两个桌子的高度非常相似。
当我们的姿势被定义后，我们可以为我们的移动组界面手臂设置目标。注意，这一次，我们调用方法 setPoseTarget（）。下面的过程与上一步相同，我们计算计划，如果成功则打印并执行移动。下面我不再展示这些线条了，因为它们在每个动作中都保持不变。
3.打开夹子
现在，我们要命令夹爪打开。至于机械臂，我们为抓手创建一个plan my_plan_gripper，并通过调用setJointValueTarget命令预配置状态“打开”。这一次我们当然使用了move_group_interface_gripper。
moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
// 3. Open the gripper
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTarge
4.将TCP移近对象
在下一步中，我们将tcp位置的前一个目标值应用于z位置的偏移量-0.2，以将机器人移向蓝框。
// 4. Move the TCP close to the object
target_pose1.position.z = target_pose1.position.z - 0.2;
move_group_interface_arm.setPoseTarget(target_pose1);
5.关闭夹子
// 5. Close the gripper
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("closed"));
6.将TCP移到板的上方
通过点击检查板的位置，我们知道，在上一步之后，它距离我们的TCP所在的蓝框的初始位置向右0.6米。所以我们只需要把它作为x轴上初始目标位置的偏移量。要将机械臂移动到板上方的位置，我们还需要沿z轴向上移动(+0.2偏移)。
// 6. Move the TCP above the plate
target_pose1.position.z = target_pose1.position.z + 0.2;
target_pose1.position.x = target_pose1.position.x - 0.6;
move_group_interface_arm.setPoseTarget(target_pose1);
7./8.下放TCP并打开夹持器
当机器人位于板上方时，我们降低TCP（-0.14）并打开夹持器。
// 7. Lower the TCP above the plate
target_pose1.position.z = target_pose1.position.z - 0.14;
move_group_interface_arm.setPoseTarget(target_pose1);
// 8. Open the gripper
move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("open"));
搞定了。这就是我们完成简单的拾取和放置任务所需的全部内容。通过在catkin工作空间中执行catkin_make来构建项目:
// 在工作空间 路径~/catkin_ws$ ，执行：
~/catkin_ws$ catkin_make
现在，您可以使用以下命令执行该任务：【手臂缓慢的开始运行，执行抓物体放物体】
// 在工作空间 路径~/catkin_ws$ ，执行：
$ rosrun ur5_simple_pick_and_place pick_and_place
8.5 Collision avoidance with Moveit
使用Moveit避免碰撞
尽管这似乎是一项非常简单的任务，但即使在这样一个简单的取放场景中，事情也可能出错。抓手需要能够抓住物体并抓住它，直到达到目标。如果物体表面非常平坦，摩擦力非常小，它可能会滑出闭合的夹持器。也许您已经在我们的演示中看到了这一点。
有时，运动规划器的最终计划会因相同的场景而不断变化。这意味着，当您为相同的起点和目标位置运行运动规划器两次时，它可能会采取不同的路径。例如，这可能导致机器人撞上其中一张桌子。
这是因为我们的运动规划器目前不知道我们环境中的任何物体。让我们通过在蓝框和盘子之间添加一个物体来说明这一点。为此，我准备了一个第二世界的拾取和放置简单碰撞，它在桌子上包含一个绿色框。因此，在ur5_gripper_moveit_config包中更改gazebo.launch文件第9行中的默认世界的名称以加载pick _ and _ place _ simple _ collision世界。该行应如下所示:
<arg name="world_name" default="$(find ur5_simple_pick_and_place)/world/simple_pick_and_place_collision"/>
当我们重新启动demo_gazebo.launch时
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch
我们看到一个绿色大盒子形状的障碍物。
现在，当我们再次执行pick_and_place ROS可执行文件时，我们看到机器人全速撞上了绿框。
8.6 Adding collision objects to the Moveit Planning Group
将碰撞对象添加到Moveit规划组
为了避免与障碍物发生碰撞，我们需要告诉Moveit关于物体的信息。所谓的规划场景代表了机器人的状态以及它周围的环境。它由move_group节点内的规划场景监视器维护。
它监听包含机器人状态的关节状态主题，并且可以接收例如来自深度相机的传感器数据。通过C++ move_group_interface，我们可以使用planning_scene主题修改世界几何信息。我们将用这个主题让计划者了解障碍。
我们在move_group初始化之后添加了添加碰撞对象的代码。我将代码添加到pick_and_place_collision.cpp文件中，您可以在 此链接 下查看。
首先，我们实例化一个planning_scene_interface(第30行),它将用于将碰撞对象添加到规划场景监视器中。然后，我们创建一个CollisionObject消息类型的实例collision_object。现在我们需要用碰撞物体的细节来填充消息。在第34行中，我们将机器人手臂的移动组接口的规划框架设置为frame_id。我们还为对象分配了一个唯一的名称。姑且称之为“box1”。
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Collision object
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_interface_arm.getPlanningFrame();
collision_object.id = "box1";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.2;
primitive.dimensions[1] = 0.4;
primitive.dimensions[2] = 0.4;
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.0;
box_pose.position.y = 0.5;
box_pose.position.z = 1.23 - 1.21;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
planning_scene_interface.applyCollisionObjects(collision_objects);
ROS_INFO_NAMED("tutorial", "Add an object into the world");
ros::Duration(2.0).sleep();
我们需要定义物体的形状。因此，创建了一个 SolidPrimitive 形状消息，并在第38行和第39行中为其分配了BOX类型。该长方体需要与碰撞世界中的长方体具有相同（或更大）的尺寸（第40至43行）。它也需要在相同的位置（第45行到第49行）。在定义了盒子的形状和姿态之后，我们将它们添加到第51行和第52行的collision_object消息中。
我们希望将碰撞对象添加到环境中，因此我们选择collision_object。ADD为要应用的操作。因为我们的planning_scene_interface 类的方法采用碰撞对象的向量，所以我们在第55行定义了一个向量，并添加了我们在第56行创建的collision_object。
现在我们可以通过调用第58行中的applyCollisionObjects方法将对象添加到规划场景中。之后，我们打印一些信息并休眠两秒钟，以确保在继续之前应用更改。
在执行结束时，我们需要将创建的碰撞对象从规划场景中移除，否则它将一直留在那里，直到我们重新开始演示。
ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);
让我们运行包含上述更改的可执行文件，看看会发生什么:
$ rosrun simple_pick_and_place pick_and_place_collision
我们可以看到，运动规划器试图在碰撞对象周围找到一条路径。蓝框和绿框之间可能仍然会有碰撞，因为规划者没有考虑被抓住的物体。这可以通过增加我们定义并添加到规划场景中的碰撞对象的尺寸来解决。
8.7 Conclusion
结论
太好了！现在我们知道了如何使用Moveit的C++接口来编写一个简单的拾取和放置任务，以及如何添加有关环境的信息，以便运动规划器可以避免碰撞。然而，关于运动规划还有很多东西需要学习。当我们确切知道环境中的物体时，避免碰撞是相当容易的。
然而，我们也可以使用传感器数据（例如来自深度相机的数据）来获取有关机器人环境的信息。然后，运动规划器也可以使用这些传感器数据进行避障。在下一个教程中，我将向您展示如何修改拾取和放置任务以使用Kinect深度相机并自动检测碰撞对象。我还创建了一个教程，向您展示如何使用OpenCV库和ROS将计算机视觉添加到拾取和放置任务中【教程十】。另一方面，有许多不同的运动规划算法可供使用。它们各有利弊。我们还将在未来的教程中讨论这些内容。在那之前，你可以在这里获得所有可用教程的概述，我很高兴在评论中收到反馈。
九、ROS Tutorial: How to use a depth camera with Moveit for collision avoidance【Motion Planning】
ROS教程九： 如何使用带有Moveit的深度相机避免碰撞
将深度相机与Moveit设置结合使用有很多优势。机器人可以避免未知环境中的碰撞，甚至可以对周围环境的变化做出反应。然而，将深度相机连接到您的设置并在您的应用程序中正确使用它可能是一个挑战。在本教程中，我将向您展示如何将Kinect摄像头添加到您的环境中，在Rviz中可视化数据并将其连接到Moveit。然后，我将向您展示如何避免碰撞以及如何在拾取和放置应用程序中正确使用深度数据。但首先，让我给你简单解释一下深度相机的实际功能。
9.1 What is a depth camera?
什么是深度相机？
使用普通相机，您可以获得特定数量的像素（取决于分辨率）以及每个像素的一些颜色信息。使用深度相机，您可以获得该像素距离相机多远的信息。这样你就知道物体到相机镜头的距离——这就是深度信息。
有了深度信息，我们就知道了物体和相机之间的距离
有不同的技术来测量每个像素的深度。一些传感器在物体上投射特定的光图案，并基于该图案如何变形来计算距离。另一方面，立体摄像机具有两个彼此之间距离已知的传感器，并通过比较来自两个传感器的数据来计算深度信息。飞行时间照相机发出的光被物体反射回来。相机接收反射光，并测量发射和接收之间的时间来计算距离。
这些不同的技术在准确性、对噪声的鲁棒性和成本等属性上各有利弊。有些更适合室内应用和短距离，而有些适合室外使用，可以看得很远。
在本教程中，我们使用微软的(模拟)Kinect摄像头，这是一个RGBD摄像头。这意味着它提供RGB彩色图像以及深度信息。
9.2 How to add the Kinect depth camera to your environment
如何将Kinect深度摄像头添加到您的环境中
为了让我们的机器人使用kinect摄像头，我们首先需要获取摄像头模型并将其添加到我们的机器人URDF文件中。然后我们需要修改我们在上一教程中创建的Moveit配置，以使用深度信息。
首先，我们从通用传感器存储库中获取Kinect模型（该存储库包含一堆ROS中常用的传感器）。继续将这个存储库克隆到您的catkin工作空间的src目录中:
~/catkin_ws/src$ git clone https://github.com/dairal/common-sensors
也许您已经从之前的教程中安装了UR5机械臂和robotiq手爪所需的Moveit配置包。如果没有，请按照快速入门部分的说明进行操作。
完成后，我们可以将Kinect URDF添加到UR5机器人手臂和抓手的URDF文件中。打开universal_robot存储库中ur_description子文件夹中的ur5_robotiq85_gripper.urdf.xacro 文件。将第81–84行添加到文件末尾的结束 < /robot>标记之前。（您还可以创建一个名为ur5_robotiq85_gripper_kinect.urdf.xacro 的新URDF，其中包含ur5_robotiq85_gripper.urdf.xacro 文件并添加Kinect传感器的线路。请记住修改moveit配置包中引用URDF路径的部分）
</gripper>
</gazebo>
<!--Kinect-->
<!-- position of kinect has to be changed in common_sensors/urdf/sensors/kinect_properties.urdf.xacro-->
<xacro:include filename="$(find common_sensors)/urdf/sensors/kinect.urdf.xacro" />
<xacro:sensor_kinect parent="world"/>
</robot>
正如include行上面的注释所说，我们可以改变传感器的位置和方向。这一点很重要，这样相机就能捕捉到我们应用程序所需的场景视图。继续操作并打开common_sensors/urdf/sensors中的kinect_properties.urdf.xacro文件。定义以下属性:
<property name="cam_px" value="0.0" />
<property name="cam_py" value="0.0" />
<property name="cam_pz" value="1.0" />
<property name="cam_or" value="0" />
<property name="cam_op" value="1.25" />
<property name="cam_oy" value="1.54" />
现在镜头指向我们的场景。最后需要配置的是Moveit配置包中的传感器参数（这已经在包中完成了）。ur5_gripper_moveit_config包的config子文件夹中的sensors_3d.yaml文件包含以下几行:
# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
- filtered_cloud_topic: filtered_cloud
max_range: 5.0
max_update_rate: 1.0
padding_offset: 0.1
padding_scale: 1.0
point_cloud_topic: /camera1/depth/points
point_subsample: 1
sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
通常没有必要知道这些参数的确切作用，所以我只给你一个简单的解释:
filtered_cloud_topic： 深度数据发布在此主题上。机器人本身已经被过滤掉了。
max_range： 测量的最大距离
max_update_rate： 八分图更新的最大速率
Padding _ offset/Padding _ scale： Padding用于补偿深度数据中的噪声和伪像。这对于机器人的自过滤尤其重要。这些参数允许调整填充比例和偏移。
point_cloud_topic： 发布原始点云数据的主题
point_subsample： 使用point_subsample点中的一个点（在我们的示例中，我们使用每个样本）
sensor_plugin： 我们用于深度数据的Moveit插件
现在我们完成了配置，并准备可视化我们的相机数据！
9.3 Visualizing depth camera data in Rviz
在Rviz中可视化深度相机数据
让我们运行以下命令打开Rviz和Gazebo中的环境:
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch
当您现在查看Rviz时，您可能还看不到任何点云数据。如果是这种情况，请单击左侧Displays section 部分底部的Add。如下图所示，从列表中选择PointCloud2，然后按确定OK。现在添加了一个名为PointCloud2的项目，您可以通过单击它左侧的箭头来展开它。在 row topic 中选择 /camera1/depth/points，然后您应该会看到数据。
现在，如果您的环境是空的，您可能仍然看不到除了一些工件之外的任何东西，但不要担心。下图显示了来自已经加载的拾取和放置场景的数据，我们将在下一步添加该场景。然而，正如您从下图中看到的那样，我们并没有获得整个环境的图像，只是从深度相机的角度和视野中可以检测到的图像。你可能还认识到Moveit会自动从点云数据中过滤掉我们的机器人。
您在Rviz中看到的网格表示称为octomap。它表示环境中被碰撞对象占据的元素。Moveit可以立即使用这个octomap进行运动规划。
9.4 Using the depth camera in a pick and place scenario
在拾取和放置场景中使用深度相机
现在我们想在拾取和放置场景中使用深度相机，我们已经在上一个教程中实现了该场景。如果您已经完成了，那么您可以跳过接下来的三个步骤。如果没有，克隆包含拾取和放置实现的存储库并构建catkin工作空间:
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_simple_pick_and_place
~/catkin_ws/src$ cd ..
~/catkin_ws/$ catkin_make
现在我们需要指定我们想要在Gazebo中加载的世界，并且我们需要对机器人的生成位置应用一个偏移量。
为了加载这个世界，我们需要修改Moveit配置包中的gazebo.launch文件。继续修改第9行，默认加载拾取和放置世界:
<arg name="world_name" default="$(find ur5_simple_pick_and_place)/world/simple_pick_and_place"/>
在这个世界中，机器人被放置在桌子上，所以我们需要修改机器人手臂产生的位置。因此，我们需要将第18行中z轴的偏移量改为桌子的高度(大约1.21米):
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 1.21"
如果Rviz和Gazebo在没有取放工具的情况下仍在运行，请关闭并重新启动它们:
$ roslaunch ur5_gripper_moveit_config demo_gazebo.launch
现在，您应该看到Gazebo中加载的拾取和放置环境，机器人坐在桌子上。目标是抓住绿色障碍物右侧的蓝色盒子，放在左侧的盘子上。如果使用以下命令运行上一教程中未修改的实现，将会遇到错误:
$ rosrun ur5_simple_pick_and_place pick_and_place_collision
它将显示一条消息，说明无法找到特定目标的运动计划。这是因为Moveit现在会检查机器人和八分图之间的碰撞。因为Moveit不知道我们要抓的蓝框，它认为移动到那个位置会导致碰撞。
为了使拾取和放置任务正常工作，我们需要修改两点:
1、我们需要将蓝盒子添加到规划场景中，以便能够取消手爪和盒子之间的碰撞检查。
2、在我们抓住物体后，我们需要将物体连接到抓手上，这样Moveit就会考虑到碰撞检查
这些更改在 ur5_simple_pick_and_place 存储库内的 pick_and_place_collision_depth_camera.cpp 文件中实施，因此您无需手动执行以下步骤。你可以在 这里 看到完整的文件。
9.4.1. Add the blue box to the planning scene and deactivate collision checking
1.将蓝框添加到规划场景中，并取消碰撞检查
在上一个教程中，我们必须将绿色障碍物添加到规划场景中，以避免与它发生碰撞。我们不再需要这样做了，因为我们的深度相机会自动检测我们环境中的碰撞对象。相反，我们定义想要抓住的对象，并将其添加到规划场景中。
collision_object.id = "blue_box";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.06;
primitive.dimensions[1] = 0.06;
primitive.dimensions[2] = 0.06;
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.3;
box_pose.position.y = 0.5;
box_pose.position.z = 1.045 - 1.21;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
planning_scene_interface.applyCollisionObjects(collision_objects);
我们定义了一个基本的形状，并在第42到47行中定义了蓝色盒子的尺寸。然后我们在第49行到第53行设置盒子的姿势。最后，我们将形状和姿态添加到我们的对象collision_object，将该对象添加到我们的collision_objects向量(第60行)，并将该向量添加到我们的规划场景(第62行)。
稍后运行代码时，我们会看到在将蓝框(在Rviz中显示为绿色)添加到场景中后，Moveit清除了它周围的octomap。
现在，Moveit知道了我们的蓝色盒子，我们可以继续禁用盒子和夹持器之间的碰撞检查。将实际抓住盒子的两个链接分别是左手和右手指尖链接。
通过LockedPlanningSceneRW类，我们可以访问第69/70行中的基础规划场景的AllowedCollisionMatrix。使用AllowedCollisionMatrix，我们可以告诉Moveit禁用某些链接/对象之间的碰撞检查。在第71行和第72行中，我们对蓝色框和左右提示链接进行了完全相同的操作。然后，我们将更改应用于原始规划场景。
planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
acm.setEntry("blue_box", "robotiq_85_left_finger_tip_link", true);
acm.setEntry("blue_box", "robotiq_85_right_finger_tip_link", true);
std::cout << "\nAllowedCollisionMatrix:\n";
acm.print(std::cout);
moveit_msgs::PlanningScene diff_scene;
ls->getPlanningSceneDiffMsg(diff_scene);
planning_scene_interface.applyPlanningScene(diff_scene);
9.4.2. Attach the object to the gripper
2.将物体附着到夹子上
当机器人抓住蓝色盒子时，我们要告诉Moveit关于附加对象的信息。这样，Moveit在计算运动时会考虑到被抓取的物体，并且不会撞上蓝框中的障碍物。
Moveit 提供 AttachedCollisionObject 消息来附加对象。我们将对象的id（在我们的例子中是“blue_box”）添加到第149行的消息中。Moveit将在我们在第150行定义的link_name 和我们的对象之间添加一个固定关节。然后，我们需要添加将与抓取的对象接触的附加链接（第151行）。最后，我们将附加的碰撞对象应用到规划场景中。
moveit_msgs::AttachedCollisionObject aco;
aco.object.id = collision_object.id;
aco.link_name = "robotiq_85_right_finger_tip_link";
aco.touch_links.push_back("robotiq_85_left_finger_tip_link");
aco.object.operation = moveit_msgs::CollisionObject::ADD;
planning_scene_interface.applyAttachedCollisionObject(aco);
在执行过程中，我们将在Rviz可视化中看到盒子在连接到抓具后的颜色变化。
就是这样！构建完成后，我们现在可以通过执行以下命令来运行任务:
$ rosrun ur5_simple_pick_and_place pick_and_place_collision_depth_camera
完整的取放任务执行(4倍速度)
当你在Rviz中查看可视化时，你可以看到机器人所看到的和计划的。在Gazebo中，你可以看到(模拟)世界中实际发生的事情。
如果你多次执行它，你可能会看到机器人有时会做出笨拙的动作(就像视频中那样)或者找不到运动计划。这是应用运动规划算法时的常见问题，处理这些问题需要对算法和规划场景有很好的了解。
9.4 Conclusion
结论
我们看到，为运动规划添加深度相机使任务更加灵活、稳健和安全，因为它可以对环境的变化做出反应。另一方面，这也增加了实现的复杂性。同样重要的是要知道，我们使用的运动规划算法在计算运动时只考虑场景中存在的对象。如果在运动过程中出现新对象，它不会调整计算的轨迹。然而，使用不同的算法是可能的。
当我们在系统中添加摄像头时，可以做更多的事情。下一篇教程将展示如何使用OpenCV库在机器人应用中使用计算机视觉。另请查看教程概述中的其他教程。我希望这篇文章对你有所帮助，我很高兴在评论区收到你的反馈！
十、ROS Tutorial: How to use OpenCV in a Robot Pick and Place task for Computer Vision【Computer Vision】
ROS教程十： 如何在计算机视觉的机器人拾取和放置任务中使用OpenCV
计算机视觉是机器人技术的重要组成部分。它帮助机器人从相机数据中提取信息以了解其环境。应用范围从提取物体及其位置到检查制造零件的生产错误，再到自动驾驶应用中检测行人。
在本文中，我将展示如何在机器人技术中使用计算机视觉来使机器人手臂执行某种程度上智能的拾取和放置任务。为此，我们将使用流行的开源库OpenCV。我们将学习如何安装OpenCV，将其连接到ROS和Kinect深度摄像头，应用计算机视觉算法提取必要的信息并将其发送给我们的机器人。这将是一篇较长的文章，我将在其中详细解释一切，以描述它从头到尾是如何工作的。对于那些只对特定主题感兴趣的人，我在开始之前整理了一份内容列表。
在本教程中，我们将讨论:
- OpenCV是什么，如何安装？
- 如何设置取放任务并运行应用程序
- 浏览OpenCV节点实现
- 使用cv_bridge包在ROS和OpenCV之间转换相机图像
- 将计算机视觉算法应用于图像
- 从图像中提取边界及其中心
- 提取物体位置
- 将2D图像像素转换为世界坐标系中的3D位置
- 将结果作为服务调用发送
- 如何从拾取和放置节点调用OpenCV节点服务
我们开始吧！
10.1 What is OpenCV and how to install it?
OpenCV是什么，如何安装？
OpenCV 最初是在英特尔开始的，后来也是由Willow Garage开发的（ROS就是在那里发明的）。它提供了一系列可移植且易于使用的优化计算机视觉算法。它是开源的，自2012年以来，非营利组织OpenCV.org接管了支持。
如果您的机器上正确安装了ROS，OpenCV 应该也已经安装了。您可以通过运行以下命令进行检查:
$ pkg-config --modversion opencv
如果这没有产生任何结果，您可以尝试:
$ dpkg -l | grep libopencv
如果您发现OpenCV尚未安装，请按照 以下链接 中的说明进行操作。
我使用ROS Melodic，它默认使用OpenCV 3，所以这是本教程中使用的版本。也可以使用更新的OpenCV 4来运行Melodic，但是需要更多的努力来运行它。其中一个额外的步骤涉及到我们用来转换图像的cv_bridge。要使用OpenCV 4，您需要从源代码中克隆并编译它。这是你会遇到的问题之一，可能还会有更多。如果您仍然想尝试用OpenCV 4设置它，您可能想从stackoverflow上的 这个 问题开始。
10.2 How to set up and run the Pick and Place task
如何设置和运行取放任务
我们希望开发我们的计算机视觉解决方案，并将其应用于真实的机器人场景。因此，我们选择在之前的教程中使用的 拾取和放置任务，在之前的教程中，【拾取和放置任务包括：】教程八：使用Moveit C++接口处理拾取和放置任务，以及 教程九：如何使用Moveit和深度相机避免碰撞。
下图显示了设置。左边的蓝色盒子应该放在右边的白色盘子上。在 【教程八】 中，我们使用Moveit的C++接口对任务进行编程，并对盒子的位置（起始位置）和盘子的位置（目标位置）进行了硬编码。我们还告诉Moveit中间绿色障碍物的确切位置和大小，以便它可以找到一条绕过它的路径。在 【教程九】 中，我们添加了一个深度相机来自动检测中间的障碍物，而无需事先告诉Moveit。现在，我们希望使用从相机获得的图像，并通过使用计算机视觉算法自动提取盒子和盘子的位置！
我们将首先进行设置并运行它，以便您可以看到程序实际做了什么。然后我将解释实现的所有细节，并带您浏览代码。
该项目包括三个主要部分:
1、 用于UR5机器人、抓手和Kinect深度摄像头的Moveit配置包。如果您完成了前面的教程，应该已经安装了这个包。如果没有，请转到 【教程七】 的快速入门部分，了解如何创建Moveit配置包并按照步骤操作。之后，查看 【避免碰撞教程九】 中关于如何将Kinect 3D摄像头添加到环境中的部分，并重复前两步。
2、 OpenCV 节点通过应用计算机视觉算法从相机图像中提取对象位置，并将它们发送到我们的应用程序。在本文后面，我将详细解释具体的实现。现在请将这个存储库克隆到您的柳絮工作区的src文件夹中：
~/catkin_ws/src$ git clone https://github.com/dairal/opencv_services.git
3. 包含【Pick and Place node】实际实现的取放节点。这将是一个新的包，但实现将与我们在前两个教程中使用的非常相似。要安装软件包，请将以下存储库克隆到您的catkin工作空间的源文件夹中：
~/catkin_ws/src$ git clone https://github.com/dairal/ur5_pick_and_place_opencv.git
现在我们已经有了所有需要的包，接下来构建您的catkin工作空间：
~/catkin_ws/src$ catkin_make
为了运行这个任务，我们需要在Linux终端中执行三个命令。首先，我们需要启动我们的环境来模拟Rviz和Gazebo中的机器人。我们通过使用两个附加参数运行ur5_gripper_moveit_config包的demo_gazebo.launch文件来实现这一点:
roslaunch ur5_gripper_moveit_config demo_gazebo.launch world_name:=$(rospack find ur5_pick_and_place_opencv)/world/simple_pick_and_place_collision robot_spawn_position_z_axis:=1.21
希望您的露台环境看起来与上面的图片相似，并且您可以在Rviz中看到可视化的深度相机数据。其次，我们启动OpenCV节点。我们要运行的节点叫做opencv _ extract _ object _ positions。通过在新终端中执行以下命令来启动它:
rosrun opencv_services opencv_extract_object_positions
除了节点的输出之外，您还会看到一个自动显示在计算机上的图像（更准确地说，是多个图像不断被一个新图像替换）。在这张图片上你已经可以看到我们的计算机视觉算法的输出。你可能会看到画在盒子和盘子中间的点。这些是我们从图像中提取的特征。opencv节点准备将提取的位置发送到我们的拾取和放置节点。最后，在我们的第三个命令中，我们启动了包含在ur5_pick_and_place_opencv包中的pick_and_place_opencv节点。因此请执行:
rosrun ur5_pick_and_place_opencv pick_and_place_opencv
现在你应该能够看到机器人在盒子上方移动，用夹子夹起它，将其移动到盘子上方并将其放在那里。手指按压，一切都为你工作。
如果你完成了之前的教程，你可能会认为这很好，但我们之前已经可以做到这一点，而无需对相机图像应用一些花哨的计算机视觉算法。没错，但现在你可以尝试在设置中修改蓝框和盘子的位置。然而，不要在这一点上太疯狂。只需将盒子和盘子从其初始位置移动一点点，并再次启动拾取和放置节点，稍后我们将讨论不能过多改变位置的原因。
我承认它不是最健壮的应用程序，有时可能会失败。需要做出更多努力，使其更加稳健和可靠。然而，实现主要是为了教育目的，我试图使它尽可能容易理解，所以让我们继续下去，我将带您浏览代码的每个细节。
10.3 Creating the OpenCV node
创建OpenCV节点
该节点将为我们提供拾取和放置任务的起点和目标位置。在我们的例子中，这是蓝框和盘子的位置。让我向您概述一下实现这一目标所需的步骤:
1、我们需要获取Kinect摄像头在我们的设置中发布的摄像头数据。这需要一些转换，因为OpenCV使用的图像格式与相机发布的图像格式不同。
2、我们将计算机视觉算法应用于图像。我会给你一个选择算法的解释，解释我为什么选择它们以及如何使用它们。
3、之后，我们将能够从图像中提取边界及其中心。
4、需要一个中间步骤来提取盒子和目标的对象位置，我将在后面进行解释。
5、在这一点上，我们有了从相机中获得的图像中2D的开始和目标位置。然而，如果我们想向机器人发送位置命令，我们需要将2D图像位置转换为机器人坐标系中的3D位置。我们将使用ROS tf2包来完成这一转换。
6、得到最终结果后，我们希望将其发送到拾取和放置节点。我们在OpenCV节点中实现了一个服务调用，可以从外部模块调用该服务调用并返回起始和目标位置。
现在您已经对必要的步骤有了一个概述，让我们开始查看实现。你可以在opencv_extract_object_positions.cpp文件中找到OpenCV node的完整代码， Github链接在此。
和往常一样，我们从主函数开始，初始化节点并创建节点处理程序:
// Main function
int main(int argc, char **argv)
{
// The name of the node
ros::init(argc, argv, "opencv_services");
// Default handler for nodes in ROS
ros::NodeHandle nh("");
10.3.1. Converting the camera images between ROS and OpenCV with the cv_bridge package
使用cv_bridge包在ROS和OpenCV之间转换相机图像
首先我们需要获取Kinect摄像头发布的摄像头数据。对于图像订阅和发布，建议使用image_transport包。其用法与订阅原始sensor_msgs/Image消息非常相似。我们包括以下:
#include <image_transport/image_transport.h>
然后我们定义一个名为IMAGE_TOPIC的常量字符串，它保存Kinect数据发布的主题名称:
static const std::string IMAGE_TOPIC = "/camera1/rgb/image_raw";
接下来，我们创建一个ImageTransport对象，它是相当于节点处理程序的image_transport，并使用它来创建订阅IMAGE_TOPIC消息的订阅者。通过第二个参数，我们定义了我们是否只想获得图像的子集（例如，10将意味着我们获得每10张发布的图像）。我们需要每个样本，所以我们定义1。第三个参数image_cb是我们在接收新图像时要调用的函数的名称。
// Used to publish and subscribe to images
image_transport::ImageTransport it(nh);
// Subscribe to the /camera raw image topic
image_transport::Subscriber image_sub = it.subscribe(IMAGE_TOPIC, 1, image_cb);
现在让我们来看看image_cb函数。ROS对sensor_msgs/Image中定义的图像使用自己的消息格式，而OpenCV使用不同的格式，实现为cv::Mat。它们是不兼容的，但是幸运的是
#include <cv_bridge/cv_bridge.h>有cv_bridge包可以为我们进行格式之间的转换，所以我们很乐意将cv_bridge包含在我们的。cpp文件:
#include <cv_bridge/cv_bridge.h>
在image_cb函数中，我们实例化了一个cv图像指针cv_ptr。然后我们调用toCvCopy函数，该函数将传入的图像消息和该消息应该转换成的编码类型作为输入参数。转换后，它返回一个cv图像指针，我们将其分配给之前创建的cv_ptr。如果抛出异常，我们会捕获它。
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
float image_size_y = cv_ptr->image.rows;
float image_size_x = cv_ptr->image.cols;
最后，我们从深度相机接收到的图像如下所示:
10.3.2. Applying Computer Vision algorithms to the images
2.将计算机视觉算法应用于图像
现在我们想从这张图像中提取信息。有不同的方法来解决这个问题。你可以把计算机视觉算法想象成一个工具箱，你可以用它来获取你想要的信息。你可以以不同的方式组合这些算法，并且通常有不止一种方法可以达到你的目标。
在 apply_cv_algorithms 函数中（第66行），我们要使用的第一个方法是灰度化/grayscaling。这使得应用进一步的算法变得更加容易。此外，我们不需要颜色信息，尽管可以通过从图像中提取某些颜色（分别为蓝色和白色）来获得盒子和盘子的位置。这是OpenCV实现灰度缩放的方式：
// convert the image to grayscale format
cv::Mat img_gray;
cv::cvtColor(cv_ptr->image, img_gray, cv::COLOR_BGR2GRAY);
OpenCV的 cvtColor 函数将图像从一种颜色空间转换到另一种颜色空间。使用第三个参数 cv::COLOR_BGR2GRAY ，我们选择转换为灰度图像，然后存储在 img_gray 对象中。
接下来，我们使用边缘检测算法来检测对象的轮廓。我们使用基于强度梯度提取边缘的 Canny algorithm 。这意味着它将检测作为边缘的相邻像素之间的显著差异。从上图中可以看出，盒子和盘子的亮度不同，但它们在背景的衬托下都很突出，这使得该算法特别适合。我们应用这样的算法:
cv::Mat canny_output;
cv::Canny(img_gray,canny_output,10,350);
cv::Canny 函数有四个参数。前两个是输入和输出图像。第三个和第四个定义阈值并影响被解释为边缘的梯度的显著性水平。尤其是第四个参数定义了这种敏感性。在我们的例子中，我们把350作为一个相当高的值。对象和背景之间的强烈对比允许我们设置如此高的值，这样做的好处是我们可以“过滤掉”中间的碰撞框。
我不会详细介绍 Canny 算法的具体工作原理，但如果你感兴趣的话，可以在 这里 找到更多信息。这是我们得到的结果：
10.3.3. Extracting boundaries and their center from the image
3.从图像中提取边界及其中心
即使最终图片中的亮区看起来像一条实线，但对计算机来说，迄今为止它只是一堆具有陡峭亮度梯度的像素。在 extract_centroids 函数中（第77行），我们现在想要连接这些点以获得对象的实际边界。因此，我们使用OpenCV的 findContours 函数，该函数简单地将具有相同亮度的所有像素连接到一条曲线上:
// detect the contours on the binary image using cv2.CHAIN_APPROX_NONE
std::vector<std::vector<cv::Point>> contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours(canny_output, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE);
让我们看看 findContours 函数的最后三个参数。我们选择轮廓检索模式 cv::RETR_TREE。如果有形状位于其他形状内部，则此参数很重要。如果是这种情况，您可能想知道它们之间的关系，即哪个形状在哪个形状内。通过第四个参数，我们选择要存储多少关于这些关系的信息。然后将结果存储在第81行中实例化的层次矩阵中。我们的对象被很好地分开，所以我们并不真正关心那部分，所以这只是给你一些背景。
通过最后一个参数，我们可以定义是否使用某种近似来存储边界，这样可以节省内存。通过设置 cv::CHAIN_APPROX_NONE ，我们不需要。生成的轮廓存储在 contours vector 中。
现在我们找到了边界，但最终我们想知道物体的位置。我们如何做到这一点？通过 计算轮廓的中心：
// get the moments
std::vector<cv::Moments> mu(contours.size());
for( int i = 0; i<contours.size(); i++ )
{ mu[i] = cv::moments( contours[i], false ); }
// get the centroid of contours.
std::vector<cv::Point2f> mc(contours.size());
for( int i = 0; i<contours.size(); i++) {
float mc_x = mu[i].m10/mu[i].m00;
float mc_y = mu[i].m01/mu[i].m00;
mc[i] = cv::Point2f(mc_x, mc_y);
}
算法并不像看起来那么复杂。在第86行中，我们循环所有找到的轮廓并计算图像矩。图像矩是特定形状中像素强度的加权平均值。根据这些力矩M，我们可以计算形状的中心（质心），如下所示:
我们对第90行到第94行的所有力矩进行计算。这就是我们为了得到物体的质心所需要做的一切。
我们用下面几条线绘制轮廓和质心，并在单独的窗口中显示它们:
// draw contours
cv::Mat drawing(canny_output.size(), CV_8UC3, cv::Scalar(255,255,255));
for( int i = 0; i<contours.size(); i++ )
{
cv::Scalar color = cv::Scalar(167,151,0); // B G R values
cv::drawContours(drawing, contours, i, color, 2, 8, hierarchy, 0, cv::Point());
cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
}
cv::namedWindow( "Contours", cv::WINDOW_AUTOSIZE );
cv::imshow( "Extracted centroids", drawing );
cv::waitKey(3);
这是我们得到的结果：
质心被绘制成小圆圈，我们可以看到我们的对象的中心被很好地检测到。然而，在图像的底部，我们可以看到其他形状的中心也被检测到。在我们得到结果之前的最后一步将是选择正确的。
10.3.4. Extracting the object positions
4.提取物体位置
为了选择正确的质心，我们使用一种简单的方法。我们假设我们的起始位置（蓝框）在图像的右上部分，目标位置（盘子）在左上部分。
为此，我编写了一个简单的函数，将质心向量和用户定义的矩形作为输入。然后计算矩形区域内所有质心的平均值。为什么我们需要计算平均值？对于同一物体，可能检测到不止一个轮廓，因此可能检测到不止一个质心。为了解决这个问题，我们简单地取平均值。额外的质心在图像中看不清楚，因为它们彼此非常接近，但它们显示在矢量中。
cv::Point2f search_centroid_in_area(std::vector<cv::Point2f> centroid_vector, cv::Rect area) {
float sum_x = 0.0;
float sum_y = 0.0;
int number_of_centroids_in_area = 0;
for( int i = 0; i<centroid_vector.size(); i++) {
if(centroid_vector[i].inside(area)) {
sum_x += centroid_vector[i].x;
sum_y += centroid_vector[i].y;
number_of_centroids_in_area++;
}
}
cv::Point2f extracted_point(sum_x/number_of_centroids_in_area, sum_y/number_of_centroids_in_area);
return extracted_point;
}
这个函数被调用两次。一次用于起始位置的搜索区域，一次用于目标位置的区域:
//get box location in 2d image
cv::Rect box_search_area((image_size_x/2), 0, (image_size_x/2), 255);
box_centroid = search_centroid_in_area(centroids, box_search_area);
//get plate location in 2d image
cv::Rect target_search_area(0, 0, (image_size_x/2), 255);
target_centroid = search_centroid_in_area(centroids, target_search_area);
我们只需选择左上角和右上角作为搜索区域：
这种方法非常简单，当然还有更复杂的解决方案。例如，我们可以尝试根据物体的形状或颜色来检测物体。
10.3.5. Converting the 2D pixel to a 3D position in the world frame
5 . 将2D像素转换为世界坐标系中的3D位置
让我们花点时间拍拍自己的肩膀。我们成功地获得了2D图像中物体的x和y坐标！但是这些和机器人有什么关系呢？最后，我们希望向机器人发出一个3D位置命令，让它选择或放置物体。因此，在我们将位置发送到拾取和放置节点之前，我们需要将2D图像中的像素位置转换为机器人框架中的3D位置。这需要两个步骤:
- 我们使用点云数据从2D像素信息中获得相机帧中的3D位置。
- 我们使用tf2包将相机帧中的3D位置转换为机器人帧。
为了 get the 3D position in the camera frame，我们需要使用来自Kinect深度相机的点云数据。因此，我们在主要功能中订阅它:
// Subscribe to the /camera PointCloud2 topic
ros::Subscriber point_cloud_sub = nh.subscribe(POINT_CLOUD2_TOPIC, 1, point_cloud_cb);
POINT_CLOUD2_TOPIC 被定义为 “/camera1/depth/points”。我们获取数据的每个样本，我们的回调函数称为 point_cloud_cb 。让我们马上来看看这个函数的第一部分:
void point_cloud_cb(const sensor_msgs::PointCloud2 pCloud) {
geometry_msgs::Point box_position_camera_frame;
pixel_to_3d_point(pCloud, box_centroid.x, box_centroid.y, box_position_camera_frame);
geometry_msgs::Point target_position_camera_frame;
pixel_to_3d_point(pCloud, target_centroid.x, target_centroid.y, target_position_camera_frame);
当我们收到一个新的点云时，我们调用 pixel_to_3d_point 函数两次，然后打印出结果。该函数将2D像素转换为相机帧中的3D位置，如下所示:
void pixel_to_3d_point(const sensor_msgs::PointCloud2 pCloud, const int u, const int v, geometry_msgs::Point &p)
{
// get width and height of 2D point cloud data
int width = pCloud.width;
int height = pCloud.height;
// Convert from u (column / width), v (row/height) to position in array
// where X,Y,Z data starts
int arrayPosition = v*pCloud.row_step + u*pCloud.point_step;
// compute position in array where x,y,z data start
int arrayPosX = arrayPosition + pCloud.fields[0].offset; // X has an offset of 0
int arrayPosY = arrayPosition + pCloud.fields[1].offset; // Y has an offset of 4
int arrayPosZ = arrayPosition + pCloud.fields[2].offset; // Z has an offset of 8
float X = 0.0;
float Y = 0.0;
float Z = 0.0;
memcpy(&X, &pCloud.data[arrayPosX], sizeof(float));
memcpy(&Y, &pCloud.data[arrayPosY], sizeof(float));
memcpy(&Z, &pCloud.data[arrayPosZ], sizeof(float));
p.x = X;
p.y = Y;
p.z = Z;
}
我们需要在点云数据中找到与2D图像像素相关的深度信息。数据存储在一个数组中，我们必须找到正确的元素。参数 pCloud.row_step 为我们提供了一行中的字节数，而 pCloud.point_step 则告诉我们一个点的字节数。
为了计算第154行中的数组位置，我们首先将变量 v 乘以 pCloud.row_step ，v是像素位置的y坐标。这意味着在数组中，我们向前跳转到包含我们的像素的行的开始位置。然后，我们将u（像素的x坐标）乘以 pCloud.point_step 以获得 arrayPosition，从而跳转到该行中正确的列。
每个3D点由三个坐标组成。每个坐标都是一个浮点值，存储在我们的字节数组中。一个浮点通常有4个字节，因此y坐标的偏移量为4，因为它存储在x坐标之后。每个坐标的确切偏移量存储在 pCloud.fields 数组中，在第157至159行中，我们将该偏移量值添加到每个坐标的 arrayPosition 中。现在，我们将数据数组中右侧位置的浮点字节数复制到坐标中。然后我们将它们存储在我们的 geometry_msgs::Point 变量p中。
在 point_cloud_cb 函数的第二部分中，我们将 3D position 从摄像机帧转换到机器人（基础）帧:
box_position_base_frame = transform_between_frames(box_position_camera_frame, from_frame, to_frame);
target_position_base_frame = transform_between_frames(target_position_camera_frame, from_frame, to_frame);
ROS_INFO_STREAM("3d box position base frame: x " << box_position_base_frame.x << " y " << box_position_base_frame.y << " z " << box_position_base_frame.z);
ROS_INFO_STREAM("3d target position base frame: x " << target_position_base_frame.x << " y " << target_position_base_frame.y << " z " << target_position_base_frame.z);
}
幸运的是，我们有tf2转换库，它使帧之间的转换变得容易。在使用转换功能之前，我们包括一些库头文件:
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
然后我们创建一个 tf_buffer 对象:
tf2_ros::Buffer tf_buffer;
在main函数中，我们创建了一个将 tf_buffer 对象作为参数的 a transform listener :
tf2_ros::TransformListener listener(tf_buffer);
现在我们准备使用转换功能。让我们看看 transform_between_frames 函数的实现:
geometry_msgs::Point transform_between_frames(geometry_msgs::Point p, const std::string from_frame, const std::string to_frame) {
geometry_msgs::PoseStamped input_pose_stamped;
input_pose_stamped.pose.position = p;
input_pose_stamped.header.frame_id = from_frame;
input_pose_stamped.header.stamp = ros::Time::now();
geometry_msgs::PoseStamped output_pose_stamped = tf_buffer.transform(input_pose_stamped, to_frame, ros::Duration(1));
return output_pose_stamped.pose.position;
}
该函数获取3D点和我们要变换的3D点之间的帧。transform函数需要 geometry _ msgs::pose stamp 类型的w位置（类似于点对象，但具有附加信息）作为输入。
我们通过添加关于帧和时间的信息，简单地从3D点构建 input_pose_stamped 。然后我们可以调用之前创建的 tf_buffer 对象的 *transform *函数。它接受构造的 input_pose_stamped、我们要转换到的帧和一些超时值。结果我们得到了转换后的输出姿态，也是 geometry _ msgs::pose stamp 类型，从中我们只返回位置。
太酷了，我们终于有了一个机器人能理解的3D位置。
10.3.6. Sending the resulting positions as a service call
6.将结果位置作为服务调用发送
请记住，我们这样做是为了自动检测拾取和放置任务的起始位置和目标位置。因此，我们需要将结果传达给我们的拾取和放置节点。到目前为止，我们主要使用主题在节点之间交换数据，这对于多对多和单向通信非常有用。但是，在开始运动之前，只有拾取和放置节点需要根据请求获取一次提取的位置。为此，服务比主题更合适。
我们将服务命名为 opencv_services/box_and_target_position，并在OpenCV包的srv文件夹中创建以下box_and_target_position.srv文件:
---
geometry_msgs/Point box_position
geometry_msgs/Point target_position
我们还需要在CMakeLists.txt文件中添加服务，以确保创建了 opencv _ node/box _ and _ target _ position . h 头文件:
add_service_files(
FILES
box_and_target_position.srv
)
在我们的.cpp文件中包含此标题后，我们就可以使用该服务了。为了调用它，我们在main函数中公布了它:
ros::ServiceServer service = nh.advertiseService("box_and_target_position", get_box_and_target_position);
当另一个节点调用服务时，将调用 get_box_and_target_position 函数:
bool get_box_and_target_position(opencv_node::box_and_target_position::Request &req,
opencv_node::box_and_target_position::Response &res) {
res.box_position = box_position_base_frame;
res.target_position = target_position_base_frame;
return true;
}
在其中，我们简单地将存储在全局 box_position_base_frame 和 target_position_base_frame 变量中的提取位置分配给我们发送回请求服务的节点的响应。
唷，那可是一大堆工作啊！在OpenCV节点中需要完成很多步骤，但现在我们终于可以将它用于我们的拾取和放置任务了。
10.4 How to call the OpenCV node service from the Pick and Place node
如何从拾取和放置节点调用OpenCV节点服务
正如我在开始时提到的，我们将使用与上一教程几乎相同的拾取和放置实现，并且只使用从OpenCV节点接收的值替换硬编码的位置值。对于本教程，我将只讨论与上一教程相比我们需要添加的行。如果您对拾取和放置任务的具体实现方式感兴趣，可以浏览最后两个教程（使用Moveit C++接口的拾取和放置任务以及如何使用带有Moveit的深度相机来避免碰撞）。您还可以在Github存储库中找到新的拾取和放置节点。 pick_and_place_opencv.cpp 文件包含完整的实现，包括描述性注释。
要使用该服务，我们需要在CMakeLists.txt中添加opencv_node作为依赖项:
find_package(catkin REQUIRED
COMPONENTS
opencv_services
...
在package.xml中添加：
...
<build_depend>opencv_node</build_depend>
...
<exec_depend>opencv_node</exec_depend>
生成的服务标头需要包含在 .cpp 文件:
8
#include "opencv_node/box_and_target_position.h"
在 pick and place 节点的主函数中，我们为 box_and_target_position 服务构建了一个服务客户端 box_and_target_position_srv_client，并在第79行创建了服务对象。现在我们调用服务，如果成功，打印出存储在 srv.response 中的接收位置。
// Get the box and the target position from the opencv node
ros::ServiceClient box_and_target_position_srv_client = n.serviceClient<opencv_node::box_and_target_position>("box_and_target_position");
opencv_node::box_and_target_position srv;
if(box_and_target_position_srv_client.call(srv)) {
ROS_INFO_STREAM("3d target position base frame: x " << srv.response.target_position.x << " y " << srv.response.target_position.y << " z " << srv.response.target_position.z);
ROS_INFO_STREAM("3d box position base frame: x " << srv.response.box_position.x << " y " << srv.response.box_position.y << " z " << srv.response.box_position.z);
} else {
ROS_INFO_STREAM("Failed to call box and target position service");
}
最后，我们将值而不是硬编码的位置分配给我们的长方体姿势:
box_pose.position.x = srv.response.box_position.x;
box_pose.position.y = srv.response.box_position.y;
box_pose.position.z = srv.response.box_position.z;
以及目标姿势:
target_pose1.position.x = srv.response.box_position.x;
target_pose1.position.y = srv.response.box_position.y;
target_pose1.position.z = srv.response.box_position.z + 0.34;
结论
就是这样，这就是如何使用OpenCV和ROS将计算机视觉集成到您的机器人应用程序中。您看到了从相机图像中提取信息并将其转换为机器人任务的有用数据需要相当多的工作。本教程中使用的方法肯定不是最优雅或最健壮的方法，但它从头到尾向您展示了一种方法，这就是目标。
OpenCV 是一个功能强大的库，还有更多的功能有待开发。在本教程中，我有意使用经典的计算机视觉算法，但越来越多的机器学习方法被用于现代计算机视觉。在下一个教程中，我们将讨论如何使用OpenCV在您的机器人应用程序中使用机器学习方法。在此之前，谢谢你一直陪伴我到最后，并确保查看机器人教程概述中的其他教程。一如既往，我很高兴在评论中收到您的反馈！
扫一扫
754
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
