描述

ROS系统下，使用moveit和gazebo搭建UR机械臂控制的仿真环境，并使用C++编写一个节点来控制UR机械臂的移动
ROS系统：kinetic
UR5机械臂
电脑系统：Ubuntu16.04

截图和运行效果

gazebo的机械臂会先水平，然后依次移动到两个位置
rviz中的机械臂移动和gazebo机械臂一致，但是会有一个透明效果的机械臂，沿着同样路径非常滞后的移动

代码

1. 特殊位置移动（Moveit中默认的位置）

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <vector>
#include <iostream>
 
int main(int argc, char **argv)
{
    
    ros::init(argc, argv, "demo"); //初始化
    ros::AsyncSpinner spinner(1); //多线程
    spinner.start(); //开启新的线程
 
    moveit::planning_interface::MoveGroupInterface arm("manipulator");//初始化需要使用move group控制的机械臂中的arm group
    
    arm.setGoalJointTolerance(0.001); //允许误差
    arm.setMaxAccelerationScalingFactor(0.2); //允许的最大速度和加速度
    arm.setMaxVelocityScalingFactor(0.2);

    arm.setNamedTarget("home"); // 控制机械臂移动到水平（躺下）
    arm.move();
    sleep(1);
 
    arm.setNamedTarget("up"); //控制机械臂到达竖直位置（竖立）
    arm.move();
    sleep(1);
 
    ros::shutdown();
 
    return 0;
}

2. 指定位置移动（欧氏空间的位置和四元数）

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <vector>
#include <iostream>
 
int main(int argc, char **argv)
{
	ros::init(argc, argv, "demo");
	ros::AsyncSpinner spinner(1);
	spinner.start();

    moveit::planning_interface::MoveGroupInterface arm("manipulator");
  
    std::string end_effector_link = arm.getEndEffectorLink(); //获取终端link的名称
    std::cout<<"end_effector_link: "<<end_effector_link<<std::endl;
   
    std::string reference_frame = "/world"; //设置目标位置所使用的参考坐标系
    arm.setPoseReferenceFrame(reference_frame);

    arm.allowReplanning(true); //当运动规划失败后，允许重新规划
    arm.setGoalJointTolerance(0.001);
    arm.setGoalPositionTolerance(0.001); //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalOrientationTolerance(0.01);   
    arm.setMaxAccelerationScalingFactor(0.2); //设置允许的最大速度和加速度
    arm.setMaxVelocityScalingFactor(0.2);

    geometry_msgs::Pose now_pose = arm.getCurrentPose(end_effector_link).pose;
    std::cout<<"now Robot position: [x,y,z]: ["<<now_pose.position.x<<","<<now_pose.position.y<<","<<now_pose.position.z<<"]"<<std::endl;
    std::cout<<"now Robot orientation: [x,y,z,w]: ["<<now_pose.orientation.x<<","<<now_pose.orientation.y<<","<<now_pose.orientation.z
       <<","<<now_pose.orientation.w<<"]"<<std::endl;
  
    arm.setNamedTarget("home");// 控制机械臂先回到初始化位置
    arm.move();
    sleep(1);

    std::vector<geometry_msgs::Pose> waypoints;
    geometry_msgs::Pose pose1;
    pose1.position.x = 0.712759;
	pose1.position.y = 0.20212;	
	pose1.position.z = 1.16729;
	pose1.orientation.x = 0.70717;
	pose1.orientation.y = 0.707044;
	pose1.orientation.z = 1.42317e-05;
	pose1.orientation.w = 7.09771e-05;
	waypoints.push_back(pose1);

    geometry_msgs::Pose pose2;
    pose2.position.x = 0.250989;
	pose2.position.y = 0.578917;	
	pose2.position.z = 1.3212;
	pose2.orientation.x = 0.707055;
	pose2.orientation.y = 0.707159;
	pose2.orientation.z = 4.39101e-05;
	pose2.orientation.w = 8.60218e-05;
	waypoints.push_back(pose2);

	// 笛卡尔空间下的路径规划
	moveit_msgs::RobotTrajectory trajectory;
	const double jump_threshold = 0.0;
	const double eef_step = 0.002;
	double fraction = 0.0;
    int maxtries = 100;   //最大尝试规划次数
    int attempts = 0;     //已经尝试规划次数

    while(fraction < 1.0 && attempts < maxtries)
    {
        fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        attempts++;
        
        if(attempts % 10 == 0)
            ROS_INFO("Still trying after %d attempts...", attempts);
    }
    
    if(fraction == 1)
    {   
        ROS_INFO("Path computed successfully. Moving the arm.");

	    // 生成机械臂的运动规划数据
	    moveit::planning_interface::MoveGroupInterface::Plan plan;
	    plan.trajectory_ = trajectory;
	    // 执行运动
	    arm.execute(plan);
        
        sleep(1);
    }
    else
    {
        ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
    }
    
	ros::shutdown(); 
	return 0;
}

3. CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(UR5_control)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  moveit_ros_planning_interface
)
find_package(Boost REQUIRED COMPONENTS system)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
)

add_executable(UR5_control_node src/UR5_control.cpp)
target_link_libraries(UR5_control_node ${catkin_LIBRARIES})

代码中你需要更改的地方

位姿点geometry_msgs::Pose pose
你可以在rviz界面，使用interactiveMarkers来拖动UR机械臂，可以把它拖动到任何一个位置，然后点击按钮Plan and Execute。这时rviz和gazebo中的机械臂会发生实际移动。
这时你可以再次运行以上程序，程序会把启动时的位姿打印出来，你照抄在代码中即可一些你需要自己设置的参数以“”括起来的字符串们问题及解决方案

1. 路径规划失败

执行这句话fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);时，fraction返回的结果是0，代表机械臂路径规划失败，fraction返回的结果是1，代表机械臂路径规划成功

我一直返回的结果就是0，也就是路径规划失败。
排查了一下，最终确认是编译过程中缺少了一个依赖包

解决方案

CMakeLists.txt文件更改
在原基础上添加了一个包moveit_ros_planning_interface

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  moveit_ros_planning_interface
)

2. Gazebo中的机械臂不动

之前我的代码并不是以上贴出来的样子，比正确的代码多了以下这句错误的话

geometry_msgs::Pose now_pose = arm.getCurrentPose(end_effector_link).pose;
waypoint.push_back(now_pose)  // 错误的话，不要添加

多了以上这句话出现的问题就是，在执行arm.execute(plan);这句话的时候，报了如下的错误

[ INFO] [1601195859.013775369, 913.641000000]: ABORTED: Solution found but controller failed during execution

实际表现是：
moveit里的机械臂动了，但是动起来是透明的效果，实体机械臂位置没有更新，gazebo里的机械臂不动。以下这张照片，中间那个机械臂就是我所说的透明的效果，图和文字没关系

问题排查过程

我一直以为，是rviz中的机械臂位置问题，如图

最开始执行代码的时候，机械臂的位置一直是"home"位置，在rviz上可以看到，机械臂的末端是处在水平面位置的，也就是可能存在和水平面的碰撞。执行代码时，rviz机械臂会以透明的效果肉眼可见的范围动一下，gazebo机械臂不动。这让我觉得是rviz认为与地面存在碰撞，而运动失败。

因此我进入了ur5_robot.urdf.xacro文件（请你前往你对应的机械臂描述文件），更改了机械臂的起始位姿。示例

<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>

改为了<origin xyz="0.0 0.0 1.0" rpy="0.0 0.0 0.0" />，这样改变的效果是，机械臂在z方向上上升了1m，也就悬在了空中，这里不做截图展示。

问题没有得到解决。

解决方案

根据我的另外一篇文章ROS下查看rviz中的机械臂位姿，我查阅了机械臂的位姿，突然发现查阅位姿这个命令的终端使用黄色字体提示了这样一句话

[ WARN] [1601210755.347884455, 0.800000000]: goalConnectCallback: Trying to add [/move_group] to goalSubscribers, but it is already in the goalSubscribers list
[ WARN] [1601210755.348000765, 0.800000000]: cancelConnectCallback: Trying to add [/move_group] to cancelSubscribers, but it is already in the cancelSubscribers list

我开始怀疑是路径规划本身的问题。

专业的讲，使用moveit能够控制机械臂运动，没有任何道理使用代码不能让机械臂运动。因此虽然代码提示规划成功，运动失败，我仍然怀疑规划出的路径并不能让机械臂成功移动。

我做了一次大胆的尝试，删除了waypoints中的第一个点，也就是当前位置的push_back。这一次成功运动了，可以运行的整段代码就在上面

后记

是stackflow上都有人说，要在waypoint中加入当前位置，我的参考书中隐隐约约也是这样写的。我现在懒得去真正落实这个问题了，出现这次调试有三个可能

那些要在waypoint中加入当前位置的，是python接口，很多人直接照搬了
moveit代码版本升级的变化
很多人互相在抄，没有跑一跑就把代码搞出来了这一次成功运动了，可以运行的整段代码就在上面