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算法速度會快不少。