ROS机械臂开发：MoveIt!中的潜规则

本篇文章介绍MoveIt!在实际开发中需要注意的一些潜规则，官方文档未提到但在实际工作中有用的一些技能。

一、圆弧轨迹规划

上一篇中介绍了直线插补，将waypoints用直线连接，而圆弧轨迹插补API是未提供的，实际中比如焊接是需要这样的轨迹的，我们自己需要实现。方法是用很多直线段模拟圆弧，取点越多越接近实际圆弧:

圆弧生成

直接看代码：

#include <math.h>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>

using namespace std;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_with_circle");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface ur5("manipulator");

    string eef_link = ur5.getEndEffector();
    std::string reference_frame = "base_link";
    ur5.setPoseReferenceFrame(reference_frame);

    ur5.allowReplanning(true);

    ur5.setGoalPositionTolerance(0.001);
    ur5.setGoalOrientationTolerance(0.01);
    ur5.setMaxAccelerationScalingFactor(0.8);
    ur5.setMaxVelocityScalingFactor(0.8);

    // 控制机械臂先回到初始化位置
    ur5.setNamedTarget("home");
    ur5.move();
    sleep(1);

    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0.70711;
    target_pose.orientation.y = 0;
    target_pose.orientation.z = 0;
    target_pose.orientation.w = 0.70711;

    target_pose.position.x = 0.070859;
    target_pose.position.y = 0.36739;
    target_pose.position.z = 0.84716;

    ur5.setPoseTarget(target_pose);
    ur5.move();

    vector<geometry_msgs::Pose> waypoints;
    waypoints.push_back(target_pose);

    //在xy平面内生成一个圆周
    double centerA = target_pose.position.x;
    double centerB = target_pose.position.y;
    double radius = 0.15;

    for(double theta = 0.0; theta < M_PI*2; theta += 0.01)
    {
        target_pose.position.x = centerA + radius * cos(theta);
        target_pose.position.y = centerB + radius * sin(theta);
        waypoints.push_back(target_pose);
    }

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

    while(fraction < 1.0 && attempts < maxtries)
    {
        fraction = ur5.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;

        // 执行运动
        ur5.execute(plan);
        sleep(1);
    }
    else
    {
        ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
    }

    // 控制机械臂先回到初始化位置
    ur5.setNamedTarget("home");
    ur5.move();
    sleep(1);

    ros::shutdown();
    return 0;
}

代码做的工作：在圆弧上均匀采样，将位置依次添加到路点，最后调用直线路径规划API实现圆弧运动。将终端轨迹显示出来如图：

圆弧运动

二、轨迹重定义

如果对规划出来的轨迹不是很满意，我们可以在plan和execute之间对轨迹进行修改。这里以删除轨迹中的中间点为例，对轨迹进行重定义。
首先调用moveit的API规划得到一条路径plan（轨迹规划）：

MoveGroupInterface::Plan plan;
MoveItErrorCode success = ur5.plan(plan);

接着调用delete_trajectory(plan, 4);对plan每隔四个点删除一个（轨迹重定义）：

void delete_trajectory(MoveGroupInterface::Plan& plan, unsigned gap)
{
    unsigned count = 0;
    auto i = plan.trajectory_.joint_trajectory.points.begin();
    while(i < plan.trajectory_.joint_trajectory.points.end())
    {
        //每gap个元素删除一次，保留首尾
        if(count % gap == 0 && count != 0)
        {
            //尾元素保留
            if(i == plan.trajectory_.joint_trajectory.points.end() - 1)
                break;
            i = plan.trajectory_.joint_trajectory.points.erase(i);
        }
        else
            i++;
        count++;
    }
}

经删除后的轨迹仍旧可以运行：

删除前31个点删除后24个点.png

三、多轨迹连续运动

如何让机械臂在各段轨迹之间不停顿，一气呵成完成所有轨迹的运动？MoveIt本身是没有此功能的，因为MoveIt规划出来的轨迹一定是从速度0到0的轨迹，我们需要重新组合API的调用来实现多轨迹连续运动。实现方法是将规划得到的多条轨迹拼接成一条，然后对这一条轨迹中的速度，加速度进行重新规划，成为一条完整的轨迹，关键就在于如何重规划。本文给出用时间最优算法对拼接的轨迹进行重规划，当然也可以用其它算法实现。

利用时间最优算法重规划

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/OrientationConstraint.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_continue_trajectory_demo");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface ur5("manipulator");

    double accScale = 0.8;
    double velScale = 0.8;
    ur5.setMaxAccelerationScalingFactor(accScale);
    ur5.setMaxVelocityScalingFactor(velScale);
    ur5.setGoalPositionTolerance(0.001);
    ur5.setGoalOrientationTolerance(0.01);

    // 控制机械臂先回到初始化位置
    ur5.setNamedTarget("home");
    ur5.move();
    sleep(1);

    std::string end_effector_link = ur5.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    ur5.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    ur5.allowReplanning(true);

    geometry_msgs::Pose target_pose;
    target_pose.position.x = -0.093;
    target_pose.position.y = 0.46713;
    target_pose.position.z = 0.84616;
    target_pose.orientation.x = 0.7071;
    target_pose.orientation.y = 0;
    target_pose.orientation.z = 0;
    target_pose.orientation.w = 0.7071;

    ur5.setPoseTarget(target_pose);

    moveit::planning_interface::MoveGroupInterface::Plan plan1;
    moveit::planning_interface::MoveItErrorCode success1 = ur5.plan(plan1);

    ROS_INFO("Plan1 (pose goal) %s", success1 ? "SUCCESS":"FAILED");

    if(success1)
      ur5.execute(plan1);
    sleep(1);

    target_pose.position.x = -0.13995;
    target_pose.position.y = 0.68947;
    target_pose.position.z = 0.64173;
    ur5.setPoseTarget(target_pose);

    moveit::planning_interface::MoveGroupInterface::Plan plan2;
    moveit::planning_interface::MoveItErrorCode success2 = ur5.plan(plan2);

    ROS_INFO("Plan2 (pose goal) %s", success2 ? "SUCCESS":"FAILED");

    if(success2)
      ur5.execute(plan2);
    sleep(1);

    // 控制机械臂回到初始化位置
    ur5.setNamedTarget("home");
    ur5.move();
    sleep(1);

    //连接两条轨迹
    moveit_msgs::RobotTrajectory trajectory;
    trajectory.joint_trajectory.joint_names = plan1.trajectory_.joint_trajectory.joint_names;
    trajectory.joint_trajectory.points = plan1.trajectory_.joint_trajectory.points;
    for(unsigned i = 0; i < plan2.trajectory_.joint_trajectory.points.size(); i++)
    {
        trajectory.joint_trajectory.points.push_back(plan2.trajectory_.joint_trajectory.points[i]);
    }

    //采用时间最优算法对轨迹重规划
    moveit::planning_interface::MoveGroupInterface::Plan joinedPlan;
    robot_trajectory::RobotTrajectory rt(ur5.getCurrentState()->getRobotModel(), "manipulator");
    rt.setRobotTrajectoryMsg(*ur5.getCurrentState(), trajectory);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    iptp.computeTimeStamps(rt, velScale, accScale);

    rt.getRobotTrajectoryMsg(joinedPlan.trajectory_);

    if (!ur5.execute(joinedPlan))
    {
        ROS_ERROR("Failed to execute plan");
        return false;
    }

    sleep(1);

    ROS_INFO("Finished");

    ros::shutdown(); 

    return 0;
}

运行效果如图，可以看出两条轨迹之间的停顿已经消除，机械臂一气呵成完成整个动作。

轨迹连接

四、更换运动学插件

通过一系列运动学插件实现正逆运动学计算，常用的有三种：

KDL求解器
来自于orocos框架，数值解，能求解封闭情况下的逆运动学，但速度慢，失败率高
Track-IK
也是数值解，但做了一些优化，速度和成功率比KDL高

安装

i5@i5-ThinkPad-T460p:~$ sudo apt-get install ros-kinetic-trac-ik-kinematics-plugin

配置
修改kinematics.yaml，将solver改为track_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

kinematics.yaml

测试

i5@i5-ThinkPad-T460p:~/ws_moveit/src/ur5_moveit_config$ roslaunch probot_anno_moveit_config demo.launch

启动过程中看到kinematics_solver加载成功

image.png

IKFast
由Rosen编写，可求解任意复杂运动链的运动学方程解析解，产生特定语言（如C++）文件供使用，稳定，速度快，最新处理器可在5us完成计算。但得到的解并非唯一，需要自己写选解的算法，一般选取与当前位置最接近的解。由于IKFast配置过程比较复杂，将在另一片文章中介绍。