前言

刚刚结束在沈阳自动化研究所的实习，实习内容是实现机械臂的轨迹规划和仿真，使用的机械臂有UR5和Panda。沈自所的机器人实力确实很强，忙活了一个假期也只是学到了一点皮毛。这篇博客也算是一个实习的记录，将我在学习ROS过程中学到的知识和遇到的坑都记录下来，也希望能为其他还在ROS里挣扎的同学们提供一些帮助。鄙人不才，这篇博客顶多算是抛砖引玉。
本文将详细的介绍ROS，从安装到仿真再到实验。因为内容较多，可能一次无法写完，会持续补充。

1. Ubuntu+ROS系统的搭建

现在ROS可以同时运行在Windows环境和Linux环境下，但是Linux环境下还是相对稳定一些，而且也更适合于程序员开发。安装Linux操作环境可以选择用虚拟机或者直接双系统（如果想要与真实的机械臂相连，不能使用虚拟机，如果一定要使用虚拟机需要打上实时补丁），在这里我选择用虚拟机来做轨迹规划仿真，用另一台装有Linux系统的电脑来做后面的真机实验。VMware是市面上目前比较主流的虚拟机工作平台，在这里我们使用它创建虚拟机。Ubuntu 16.04是目前和ROS Kinetic版本兼容度最高的，鉴于我们之后会使用ROS Kinetic版本（因为这是比较稳定而且较新的版本），我们在虚拟机中安装Ubuntu 16.04版本。

a) 在VMware虚拟机中安装Ubuntu 16.04；

1. 下载并安装VMware v12.1.0，下载Ubuntu 16.04镜像文件

2. 运行VMware v12.1.0，创建新的虚拟机，遵循下面这个网页教程，可以完成后续安装，这里就不再赘述。（https://www.jianshu.com/p/3379892948da）

3. 再安装好虚拟机和Ubuntu 16.04之后，我们重启虚拟机，往往会出现屏幕尺寸太小的情况。有以下两种方法可以解决：

   1. 第一种方法，在终端输入xrandr，并执行，输入我们需要设置的分辨率，xrandr -s 1920x1440，然后执行。
   2. 第二种方法，直接打开虚拟机的设置，更改屏幕分辨率再应用

b) 在Ubuntu16.04LTS 中安装ROS（版本Kinetic）:

1. 在控制台中输入命令：sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

2. 再输入命令：sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116

3. 开始正式的安装，安装豪华至尊版ROS：sudo apt-get update

4. 前几步一般不会有太大问题，安装完成后可以查看使用的包：apt-cache search ros-kinetic

5. 到此，还没有结束，需要初始化：sudo rosdep init

6. rosdep update（这一命令会把所有相关的依赖项都升级到最新版）
   (如果出现无法运行rosdep的情况，用rospack find rosdep检查rospack是否安装，用sudo apt install rospack-tools命令安装，但要注意的是，出现这种情况很有可能第2步执行安装命令出错，应该重新执行第2步）

7. 配置环境变量：echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
   (注意：kinetic的k是小写，如果出现“没有找到文件”的错误，可以通过gedit .bashrc来查看最后一行的source文件是否是小写k, 这里非常坑，很多教程都写错了)

8. source ~/.bashrc

9. 此时，就完成了安装，可以测试一下：在命令行终端中输入roscore并运行。
   此时如果出现：
   
   那么恭喜你，ROS成功安装上了

10. 我们可以跑一个测试程序—小海龟。先安装示例 ：sudo apt-get install ros-kinetic-turtlesim (16.04版本可能不用这一步也能直接跑)
    然后，在三个不同的终端分别执行以下三个指令：
    roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
    然后你就会看到经典的小乌龟窗口：
    
    试试用上下左右建来控制小乌龟吧~

11. 在正式使用ROS进行开发之前，我推荐大家几个插件，会在后面的开发时给大家节省大量的时间。
    a). sudo apt install terminator（这是一个非常好用的终端插件，可以随意的分屏，鉴于我们平时使用ROS经常需要终端多开，可以随意分割窗口还是非常方便的）
    b). sudo apt install python-pip （使用Python语言脚本进行开发的话，一定会用到的插件）
    c). sudo pip install ipython （可以实现对仿真进行同步debug，因为有的时候程序本身没有错，但是结合仿真就会跑飞）
    d). sudo apt install meld （一个文本比较工具，当你修改过某个文件夹下的多个文件，但是又想不起来修改过哪些时，会派上用场）

2. ROS工作原理与Node通讯机制：

在教大家如何使用ROS控制机械臂之前，我们先来了解一下ROS的基本知识：

a) 首先，什么是ROS:

ROS（Robot Operating System）是一个开源平台，集成了各种各样的服务，包括视觉识别，轨迹规划，模型仿真等很多强大的开发功能。

b) ROS通过Package来管理所需的文件，通常一个ROS Package包含以下文件或文件夹：

1. Launch文件夹：包含所有launch文件
2. Src文件夹：包含所有的cpp文件和python文件
3. CMakeLists.txt：包含所有需要执行的cmake配置
4. package.xml: 包含所有package信息和依赖项

c) ROS nodes（节点）

nodes是ROS里的一个基础程序，它是一个可执行文件，通过ROS与其他的nodes进行通讯。Nodes可以向topic发布或提取信息，可以提供或使用一个Service。

这里有个小技巧，我们自己创建node的时候，可以通过rosnode list来查看当前有哪些nodes在运行，如果我们创建的node不在列表里，那么说明我们的node可能没有运行起来，需要重新source一下。

d) ROS message

message是node在向topic发布或提取信息时的一种消息格式

可以使用rosmsg show 来查看某一种特定message的格式，因为message通常都是自定义的一种格式或已经定义好的一种格式，类似于C语言中的结构体，通常包含多个不同类型的变量。

e) 三种实现nodes间通信的方法

1. Ros topic：message通过publisher和subscriber来传递于多个nodes之间，而topic类似于一个公告板，所有的nodes可以通过topic实现相互通信。一个topic可能会有很多publisher和subscriber，而一个node也可能会从多个topic上发布或获取message。Topic是用来表示message内容的名称。
   比如：
   
   左边的teleop_turtle和右边的turtlesim是两个ROS nodes，他们通过中间的topic“/turtle1/command_velocity”进行通讯。两个node是无法直接进行通讯的，通过发布器node发布message给topic，接收器再从topic上获取有用的信息，所以这种通讯通常不是同步的。
2. ROS service: service 是另一种可以实现nodes之间相互通信的方式。于topic的方法不同在于，topic使用publisher和subscriber这种非常灵活的传递信息方式，但这种多对多，单向的灵活传递信息的方式，并不适用于分布式系统里需要答复请求的场景。所以衍生出了service的通信方式, service由一对message定义：一个用于请求，一个用于回复。当一个ROS node提供service通信时，客户端会发送一个请求给这个node，并停止动作等待回复。
3. ROS action: Action 是第三种可以实现nodes之间相互通信的方式。Action和Service的区别在于，Service是同步的，当一个ROS程序调用一个service的时候，程序会停止当前的运行直到收到service的答复。而Action是不同步的，这就像启动一个新线程。当ROS程序调用action时，ROS程序可以在当前线程中等待action的答复，但在另一个线程中执行其他任务。

f) 什么是launch文件

还记得我们最开始时运行的小乌龟实例程序么？让我们来看看它的launch文件。

<launch><!-- turtlebot_teleop_key already has its own built in velocity smoother --><node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard"  output="screen"><param name="scale_linear" value="0.5" type="double"/><param name="scale_angular" value="1.5" type="double"/><remap from="turtlebot_teleop_keyboard/cmd_vel" to="cmd_vel_mux/input/teleop"/></node>
</launch>

这里面node那一行是最重要的，我们先忽略其他的。
node那一行包含四个重要信息：

1. pkg=“package_name” # Name of the package that contains the code of the ROS program to execute
2. type=“cpp_executable_name” # Name of the cpp executable file that we want to execute
3. name=“node_name” # Name of the ROS node that will launch our C++ file
4. output=“type_of_output” # Through which channel you will print the output of the program

后面我们会详细介绍如何创建launch文件。

g）如何获取topic和message的信息

在ROS里，可以用rostopic list命令来获取所有可用的topic，同时也可以用rostopic echo <topic_name>命令来查看对应topic正在发布的消息，rostopic echo <topic_name> -n1可以获取对应topic发布的最后一条消息。

使用rostopic info <topic_name>命令可以查看对应topic的信息

如果想获得某一种message的信息，可以使用rosmsg show <message>命令，比如：rosmsg show std_msgs/Int32

这里的Int32是type，不过用法有点儿像struct

通过rostopic info <topic_name>可以知道你编写的Python文件需要发布什么类型的数据来控制机械臂，再用rosmsg show <message>来确定variable的名称
rostopic pub <topic_name> <message_type> <value>，这个命令可以用来立即发布一些你想要发布的指令，用来测试subscriber是否在正常运行，例如：rostopic pub /counter std_msgs/Int32 7 , 这个命令可以让counter向screen持续发送数字‘7’

重要总结：在使用ROS控制仿真时，如果能找对应的topic的名字，可以使用rostopic info <topic_name>命令来查看对应的msg的格式，然后再通过rosmsg show <message>命令来获取对应msg的组成部分，可以很容易的找到我们需要的数据组成，因为msg就像是一个结构体，而我们通常只需要结构体中的一小部分数据。在找到对应数据的调用格式之后，我们可以用rostopic echo <topic_name>命令来直观的看到对应topic的数据返回值的样子，这样有助于编写Python代码。当写完Python代码之后，就可以写launch文件和CMakelist文件了，这样一个完整的ROS包就算是初步组装完成可以运行啦~

3. 创建workspace（工作区）

ROS对机械臂的所有操作都是在一个工作区内实现的，所以我们先创建工作区。

1.  mkdir -p ~/XXX_ws/src   (这里的‘XXX’可以起自己喜欢的名字)cd ~/catkin_ws/catkin_makesource devel/setup.bash
   

(有的教程里还有下面这句，但是我自己测试发现好像没什么用，大家要是有什么发现请告诉我)
echo $ROS_PACKAGE_PATH /home/youruser/catkin_ws/src:/opt/ros/kinetic/share (此处的"youruser"请修改为自己的用户名，也就是创建虚拟机时的那个)

4.创建一个ROS Package

1. cd ~/catkin_ws/src  (打开我们刚刚创建的工作区)
   catkin_create_pkg <包的名字><包的依赖包>   (包的名字随便起，如果是用CPP编程，依赖包就写roscpp，如果是Python编程，就是rospy)
   

2. 我用的是Python，所以下面我们按Python来建包：
   catkin_create_pkg my_package rospy
3. 可以用rospack list | grep my_package，或roscd my_package来确认是否成功创建包。
   注意：此处的my_package是刚刚创建的ROS包的名字，找不到的话用命令行：
   source /home/muyang/Muyang_ws/devel/setup.bash
   或者使用
   sudo gedit ~/.bashrc
   在最后一行添加
   export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/你的工作空间路径/src
   注意：重启terminal生效
4. 下一步，回到catkin_ws 文件夹下，运行catkin_make
   注意：当workspace下包含多个package时，可以用catkin_make --only-pkg-with-deps 你的ROS包对刚刚创建的‘‘你的ROS包’’进行catkin_make，有一个特殊的语句catkin_make -DCATKIN_WHITELIST_PACKAGES=“你的ROS包”，和前面的命令行一样，都是只运行指定的package

5. MoveIT控制机械臂：

1. 建立机械臂仿真模型：

a) 第一步建立工作区，生成文件夹，mkdir -p ~/Muyang_ws/src，这里我把自己的工作区起名为“Muyang_ws”
b) cd ~/ Muyang _ws/，打开刚刚生成的文件夹
c) catkin_make，这步很关键，是在生成相关的配置文件，包括Cmakelist.txt等
d) source devel/setup.bash，通过这个命令，我们就可以在ros中调用这个工作区下的文件了。
e) 第一步：使用sudo apt-get install ros-kinetic-moveit命令安装moveit，moveit是我们建立仿真模型的软件
f) 在第一步的基础上，我们使用sudo apt-get install ros-kinetic-franka-description命令安装panda机械臂的urdf
g) 通过roslaunch moveit_setup_assistant setup_assistant.launch命令，启动MoveIt! Setup Assistant，会出现下面这样的窗口：

h) 我们选择Create New MoveIt! Configuration Package，在弹出的页面中点browse，选择路径/opt/ros/kinetic/share/franka_description/robots/panda_arm_hand.urdf.xacro ，确认后点load会出现下面这样的窗口：

i) 下一步，配置self-collision，直接选择默认的95%，点击Generate Collision Matrix：

j) 下一步，定义Virtual Joints，像这样：

k) 接下来，定义Planning Groups，配置如图：

l) 点Add Joints，如下图选择这些joint：

m) 点击save，再点击add group，然后如下配置：

n) 注意，接下来不用点add joints，我们点击Add Links，在弹出的窗口里选择如下links：
o) 接下来我们给机械臂设定几个特定的动作，选Robot poses， 点击add pose，调节8个joints，不用刻意调整数值，让姿态是直立就好，这一步主要是为了检查前几步是否正确:

p) 接下来定义End Effectors，配置如下：

q) 鉴于我们没有加入深度相机，我们直接跳到ROS Control的部分，配置如下：

点击Add planning group joints，选择panda_arm：

r) 在Author information里，填入作者姓名和邮箱，注意这里必须填写，否则无法生成仿真模型。最后在Configuration files里找到我们之前生成的工作区，点击Generate Package：

2. 配置实际机械臂：

a) 到目前为止，仿真模型已经配置好了，我们可以选择用运行demo.launch文件进行仿真，也可以选择自己配置一个新的文件来运行仿真，鉴于我们后面希望将仿真于真机连接起来，而demo.launch是做不到的，所以我们来自己配置一个运行环境。
b) 首先，在刚刚生成的panda_moveit_config下找到config文件夹，创建controllers.yaml文件，在文件内填写如下格式的命令行：

controller_list:- name: panda_arm_controlleraction_ns: execute_trajectorytype: ExecuteTrajectoryjoints:- panda_joint1- panda_joint2- panda_joint3- panda_joint4- panda_joint5- panda_joint6- panda_joint7- panda_joint8- name: hand_controlleraction_ns: pickuptype: Pickupjoints:- panda_finger_joint1- panda_finger_joint2- panda_hand_joint

c) 然后再同样的config文件夹下，新建“joint_names.yaml”文件，将上一步中所有的joint都填写进去，格式是：

controller_joint_names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, panda_joint8, panda_finger_joint1, panda_finger_joint2, panda_hand_joint]

d) 然后在launch文件夹下，修改panda_moveit_controller_manager.launch文件，注意：这里的panda根据不同的机械臂模型可能是不一样的，比如后面用到ur5机械臂时会修改为ur5。修改文件夹的内容为：

<launch>
<rosparam file="$(find panda_moveit_config)/config/controllers.yaml"/>
<param name="use_controller_manager" value="false"/>
<param name="trajectory_execution/execution_duration_monitoring" value="false"/>
<param name="moveit_controller_manager" 
value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
</launch>

e) 然后同样在launch文件夹下，新建panda_planning_execution.launch文件，文件内容格式为：

<launch><arg name="sim" default="true"/><rosparam command="load" file="$(find panda_moveit_config)/config/joint_names.yaml"/><include file="$(find panda_moveit_config)/launch/planning_context.launch" ><arg name="load_robot_description" value="true" /></include><group if="$(arg sim)"><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"><param name="/use_gui" value="false"/><rosparam param="/source_list">[/joint_states]</rosparam></node></group>
<--!此处的group的部分是用来仿真时用的，连上真实机械臂的时候，需要删掉。--><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /><include file="$(find panda_moveit_config)/launch/move_group.launch"><arg name="publish_monitored_planning_scene" value="true" /><arg name="allow_trajectory_execution" value="true"/><!--arg name="fake_execution" value="true"/--><arg name="info" value="true"/></include><include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"><arg name="config" value="true"/></include></launch>

6. 编写Python文件

1. 在my_package下的src文件夹下新建file，取名为planning_script.py

#!/usr/bin/env python# Author: Francisimport sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from moveit_msgs.msg import RobotState, Constraintsmoveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
robot = moveit_commander.RobotCommander()scene = moveit_commander.PlanningSceneInterface()group_name = "panda_arm"
group = moveit_commander.MoveGroupCommander(group_name)display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=1)def wait_for_state_update(box_is_known=False, box_is_attached=False, timeout=4):box_name = "box"start = rospy.get_time()seconds = rospy.get_time()while (seconds - start < timeout) and not rospy.is_shutdown():attached_objects = scene.get_attached_objects([box_name])is_attached = len(attached_objects.keys()) > 0is_known = box_name in scene.get_known_object_names()if (box_is_attached == is_attached) and (box_is_known == is_known):return Truerospy.sleep(0.1)seconds = rospy.get_time()return Falsedef creat_box(scene,group,pose=[]):rospy.sleep(2.0)box_pose = geometry_msgs.msg.PoseStamped()box_pose.header.frame_id = "panda_link0"box_pose.pose.orientation.w = pose[0]# box_pose.pose.orientation.x = 0.0# box_pose.pose.orientation.y = 0.0# box_pose.pose.orientation.z = 0.0box_pose.pose.position.x = pose[1]box_pose.pose.position.y = pose[2]box_pose.pose.position.z = pose[3]box_name = "box"scene.add_box(box_name, box_pose, size=(pose[4], pose[5], pose[6]))wait_for_state_update(box_is_known=True, timeout=5)print "============ Printing robot state"print robot.get_current_state()print ""def go_to_pose(robot, group, pose=[]):pose_goal = geometry_msgs.msg.Pose()pose_goal.orientation.x = pose[3]pose_goal.orientation.y = pose[4]pose_goal.orientation.z = pose[5]pose_goal.orientation.w = pose[6]pose_goal.position.x = pose[0]pose_goal.position.y = pose[1]pose_goal.position.z = pose[2]group.set_pose_target(pose_goal)group.go(wait = True)print "============ Printing robot state"print robot.get_current_state()print ""creat_box(scene,group,[1.0, 0.2, 0.2, 0.25, 0.1, 0.1, 0.5])go_to_pose(robot, group, [0.30603, 0.017247, 0.64808, 0.59731, 0.52117, -0.4175, -0.44418])
# rospy.sleep(10.0)
go_to_pose(robot, group, [0.090837, 0.42689, 0.19629, 0.92343, 0.38265, -0.026938, -0.011112])# result = group.go(wait=True)group.stop()group.clear_pose_targets()rospy.sleep(10)# moveit_commander.roscpp_shutdown()

2. 创建launch文件，在my_package 文件夹下执行mkdir launch，再执行touch launch/my_package_launch_file.launch。在IDE中编写launch文件：

<launch><!-- My Package launch file --><node pkg="my_package" name="move_group_python_interface_tutorial"  type="planning_script.py" output="screen"></node>
</launch>

3. 最后一步，为了能让刚刚新建的Python文件能执行，要修改它的执行权限，运行命令行chmod u+x planning_script.py

7. 仿真与实验

a) Panda机械臂的仿真:

1. 打开终端，运行roscore
2. 新建终端运行前面第5部分创建的panda_planning_execution.launch文件新建终端运行前面第5部分创建的panda_planning_execution.launch文件
3. 再新建终端运行前面第6部分创建的my_package_launch_file.launch文件。
4. 仿真效果如下：
   

b) UR5机械臂的仿真与实验
（这部分是后加的，实习计划里本来没有这一项，但是因为提前完成了实习任务，所里又正好有空闲的机械臂，所以临时增加了实习内容，鉴于所里目前只有UR5机械臂可以供我使用，所以在真实机械臂上运行的轨迹规划是在UR5上实现的，3.8版本）

1. 仿照前面在创建my package的ROS包之后，从github上下载ur_modern_driver, 网址是：https://github.com/ros-industrial/ur_modern_driver 和universal_robot，https://github.com/ros-industrial/universal_robot
2. 跟随http://wiki.ros.org/rosdep的步骤，运行sudo apt-get install python-rosdep，用sudo rosdep init初始化rosdep
3. 使用rosdep install --from-paths src --ignore-src -r -y 下载相关依赖包。完成时用catkin_make编译。注意：运行时需要将机器人通过网线与计算机连接到一起
4. 因为所里的UR5是3.8版本的，所以需要修改之前下载的ur_modern_driver下的文件
   1. 修改ur_modern_driver下的src/robot_state_RT.cpp文件：在340行加上几行代码，修改对版本的支持。见第5条
   2. 修改ur_modern_driver下的include下的ur_modern_driver.h下的ur_hardware_interface.h文件，把canSwitch函数声明改成prepareSwitch, 把函数内的const尾缀删掉。
   3. 修改ur_modern_driver下的Cmakelist.txt，注释掉catkin_package的最后一项DEPENDS
   4. catkin_make刷新工作区
   5. 需要添加的程序行

else if (version_ >= 3.3 && version_ < 3.5) {if (len != 1060)len_good = false;} else if (version_ >= 3.5 && version_ <=3.8) {if (len != 1108){len_good = false;}
}

5. 仿照第六部分, 在Muyang_ws下的src文件夹下创建新的名为ur5_package的 ROS包。在此文件夹下创建新的Python文件:

#!/usr/bin/env pythonimport sys
import rospy
import moveit_commander
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState, Constraints
import geometry_msgs
from geometry_msgs.msg import Pose
from moveit_commander import MoveGroupCommander
import copyif __name__=='__main__':roscpp_initialize(sys.argv)rospy.init_node('moveit_py_demo', anonymous=True)robot = RobotCommander()rospy.sleep(1)group = MoveGroupCommander("manipulator")# group.set_start_state(RobotState())scene = moveit_commander.PlanningSceneInterface()def wait_for_state_update(box_is_known=False, box_is_attached=False, timeout=4):# Copy class variables to local variables to make the web tutorials more clear.# In practice, you should use the class variables directly unless you have a good# reason not to.box_name = "box"## BEGIN_SUB_TUTORIAL wait_for_scene_update#### Ensuring Collision Updates Are Receieved## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^## If the Python node dies before publishing a collision object update message, the message## could get lost and the box will not appear. To ensure that the updates are## made, we wait until we see the changes reflected in the## ``get_known_object_names()`` and ``get_known_object_names()`` lists.## For the purpose of this tutorial, we call this function after adding,## removing, attaching or detaching an object in the planning scene. We then wait## until the updates have been made or ``timeout`` seconds have passedstart = rospy.get_time()seconds = rospy.get_time()while (seconds - start < timeout) and not rospy.is_shutdown():# Test if the box is in attached objectsattached_objects = scene.get_attached_objects([box_name])is_attached = len(attached_objects.keys()) > 0# Test if the box is in the scene.# Note that attaching the box will remove it from known_objectsis_known = box_name in scene.get_known_object_names()# Test if we are in the expected stateif (box_is_attached == is_attached) and (box_is_known == is_known):return True# Sleep so that we give other threads time on the processorrospy.sleep(0.1)seconds = rospy.get_time()# If we exited the while loop without returning then we timed outreturn Falserospy.sleep(2.0)box_pose = geometry_msgs.msg.PoseStamped()box_pose.header.frame_id = "world"box_pose.pose.orientation.w = 1.0box_pose.pose.orientation.x = 0.0box_pose.pose.orientation.y = 0.0box_pose.pose.orientation.z = 0.0box_pose.pose.position.x = -0.04482box_pose.pose.position.y = -0.4box_pose.pose.position.z = 0.56438box_name = "box"scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.2))wait_for_state_update(box_is_known=True, timeout=5)# print("wait for state update")# start to movegroup.set_start_state_to_current_state()# print "current pose:"# print group.get_current_pose()c = Constraints()waypoints = []waypoints.append(group.get_current_pose().pose)# Move forwardwpose = Pose()wpose.position.x = 0.25659wpose.position.y = -0.34674wpose.position.z = 0.62301wpose.orientation.x = 0.58494wpose.orientation.y = -0.41463wpose.orientation.z = -0.49934wpose.orientation.w = 0.48641waypoints.append(copy.deepcopy(wpose))# middle point wpose.position.x = -0.032889wpose.position.y = -0.21751wpose.position.z =  0.77293wpose.orientation.x = 0.58496wpose.orientation.y = -0.41468wpose.orientation.z = -0.4993wpose.orientation.w = 0.48637waypoints.append(copy.deepcopy(wpose))# move up# wpose.position.x = -0.04482# wpose.position.y = 0.18196# wpose.position.z = 0.56438# wpose.orientation.x = -0.70709# wpose.orientation.y = 7.9492e-05# wpose.orientation.z = 4.5739e-06# wpose.orientation.w = 0.70713# waypoints.append(copy.deepcopy(wpose))# Move down# wpose.position.z -= 0.10# waypoints.append(wpose)wpose.position.x = -0.40715wpose.position.y = -0.44703wpose.position.z =  0.5731wpose.orientation.x = 0.58481wpose.orientation.y = -0.41475wpose.orientation.z = -0.49934wpose.orientation.w = 0.48645waypoints.append(copy.deepcopy(wpose))# Move to the side# wpose.position.y += 0.05# waypoints.append(wpose)# plan, fraction = group.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c)group.set_planning_time(20)# for i in range(10):# plan, fraction = group.compute_cartesian_path(waypoints, 0.01, 0.0, avoid_collisions=True)# print 'Plan success percent: ', fraction# if fraction >= 0.9:#     breakgroup.set_pose_target(waypoints[0])plan1 = group.plan()group.go(wait=True)group.set_pose_target(waypoints[1])plan2 = group.plan()group.go(wait=True)group.set_pose_target(waypoints[2])plan3 = group.plan()group.go(wait=True)group.set_pose_target(waypoints[3])plan4 = group.plan()group.go(wait=True)group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()group.clear_pose_targets()

5. 用网线连接真机之后，运行轨迹规划：
   a. 运行roslaunch ur_modern_driver ur5_ros_control.launch limited:=true robot_ip:=“192.168.0.10”，这里的robot ip可以通过在终端内用ping的方式来获得
   b. 新开一个终端，运行roslaunch panda_moveit_config mypanda_planning_execution.launch limited:=true
   c. 再新开一个终端，运行刚刚新建的Python文件
6. 运行结果：
   完成以上步骤，UR5机械臂就会按照轨迹规划的多个点进行移动，但是要注意的是，虽然机械臂会按照规划好的空间点按照顺序移动，但是因为反运动学解析的算法原因，有些点可以对应多个姿态，这种情况会可能会导致机械臂大幅度的摆动，容易造成事故，所以在真机上运行时，一定要确保急停键待命。
   仿真和真机运行结果: