RobWork框架编程（4）：对机器人的当前状态进行碰撞检测

碰撞检测功能在机器人的运动规划技术中是必不可少的，毕竟机器人所处的真实环境是如此的复杂，而我们却要获取机器人无碰撞的运动轨迹。
运动规划算法通常比较流行的有两大类：一类是基于采样的运动规划算法，如大名鼎鼎的OMPL运动规划库，另外一类就是各种基于优化的运动规划算法，如伯克利开源的TrajOpt算法，根据其论文的描述，该算法使用了顺序凸优化技术。当然了，还有结合采样和优化的运动规划算法，但是通常我们主要把它归类为基于优化的运动规划算法。通过基于采样的运动规划算法获取一条比较粗糙的可行路径，然后以此为初始条件进行优化，从而得到比较理想的运动轨迹。

提供碰撞检测功能的第三方库有很多，比如常用的有PQP、FCL、Bullet、YAOBI等等。
如果你对碰撞检测相关的研究比较感兴趣，建议访问 awesome-collision-detection 。

在本篇博客中，我们主要介绍如何基于RobWork框架编程实现对机器人当前状态的碰撞检测，这在机器人的路径规划中是必不可少的步骤。

#include <iostream>
#include <vector>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/loaders/WorkCellFactory.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/models/Device.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/common/Ptr.hpp>
#include <rw/math.hpp>
#include <rw/pathplanning/StateConstraint.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyFactory.hpp>
#include <rw/invkin/JacobianIKSolver.hpp>
#include <rw/invkin/IKMetaSolver.hpp>


using namespace rw::common;
using namespace rw::models;
using namespace rw::loaders;
using namespace rw::kinematics;
using namespace rw::math;
using namespace rw::proximity;
using namespace rwlibs::proximitystrategies;
using namespace rw::pathplanning;
using namespace rw::invkin;

int main() {
    std::cout << std::boolalpha;
    std::string filename = "/home/liuqiang/UR3_2015/UR3.xml";
    WorkCell::Ptr workcell = WorkCellLoader::Factory::load(filename);
    std::cout << workcell->getFilename() << "\n";
    std::cout << workcell->getFilePath() << "\n";
    Device::Ptr device = workcell->findDevice("UR3_2015");
    State state = workcell->getDefaultState();
    //
    CollisionDetector::QueryResult result;
    CollisionDetector::Ptr detector = ownedPtr(new CollisionDetector(workcell, ProximityStrategyFactory::makeDefaultCollisionStrategy()));
    //
    StateConstraint::Ptr state_constraint = StateConstraint::make(detector);
    //Q[6]{1.568, -2.4, 1.825, -0.456, -1.571, 0}
    Q q1(6, 1.568, -2.4, 1.825, -0.456, -1.571, 0); //collision free
    device->setQ(q1, state);
    detector->inCollision(state, &result);
    std::cout << "collision free:\n";
    std::cout << "result.collidingFrames.size(): " << result.collidingFrames.size() << "\n";
    if (result.collidingFrames.size() > 0) {
        for(const FramePair &pair : result.collidingFrames) {
            std::cout << "Colliding: " << pair.first->getName() << " -- " << pair.second->getName() << std::endl;
            Log::infoLog() << "Colliding: " << pair.first->getName() << " -- " << pair.second->getName() << std::endl;
        }
    }
    //Q[6]{1.568, -2.331, 2.224, 0.204, -1.316, 0}
    Q q2(6, 1.568, -2.331, 2.224, 0.204, -1.316, 0); //in collision
    device->setQ(q2, state);
    Transform3D<> tcp_pose = device->baseTend(state);
    JacobianIKSolver::Ptr solver = ownedPtr(new JacobianIKSolver(device, state));
    solver->setSolverType(JacobianIKSolver::SVD);
    solver->setMaxIterations(50); //如果不设置，默认是迭代20次
//    IKMetaSolver::Ptr meta_solver = ownedPtr(new IKMetaSolver(solver, device, detector));
//    meta_solver->setMaxAttempts(50);
//    auto solutions = meta_solver->solve(tcp_pose, state);
    auto solutions = solver->solve(tcp_pose, state);
    std::cout << "solutions.size(): " << solutions.size() << "\n";
    std::cout << solutions[0] << "\n";
    std::cout << device->getQ(state) << "\n";
    std::cout << "in collision:\n";
    std::cout << "state_constraint->inCollision(state): " << state_constraint->inCollision(state) << "\n";
    detector->inCollision(state, &result);
    std::cout << "result.collidingFrames.size(): " << result.collidingFrames.size() << "\n";
    if (result.collidingFrames.size() > 0) {
        for(const FramePair &pair : result.collidingFrames) {
            std::cout << "Colliding: " << pair.first->getName() << " -- " << pair.second->getName() << std::endl;
            Log::errorLog() << "Colliding: " << pair.first->getName() << " -- " << pair.second->getName() << std::endl;
        }
    }
    return 0;
}

运行输出的结果为：

/home/liuqiang/UR3_2015/UR3.xml
/home/liuqiang/UR3_2015/
collision free:
result.collidingFrames.size(): 0
solutions.size(): 1
Q[6]{1.568, -2.331, 2.224, 0.204, -1.316, 0}
Q[6]{1.568, -2.331, 2.224, 0.204, -1.316, 0}
in collision:
state_constraint->inCollision(state): true
result.collidingFrames.size(): 1
Colliding: UR3_2015.Flange -- UR3_2015.Joint1
Colliding: UR3_2015.Flange -- UR3_2015.Joint1

测试的collision free状态如下截图：
而测试的in collision状态如下截图：

从我们运行输出的结果也可以看出，机器人在collision free状态下是没有碰撞，而在in collision状态下UR3_2015.Flange 和 UR3_2015.Joint1之间发生了碰撞。

总结：本篇博客主要介绍如何通过代码实现机器人状态的碰撞检测功能。实现了碰撞检测功能之后，就可以进行机器人的运动规划了，比如自己实现一个基于采样的RRT算法来进行测试，不过需要说明的是基本的RRT算法速度很慢，改进版的双向RRT算法速度会快不少。