机器人C++库(10)Robotics Library 之碰撞检测算法
1.碰撞检测开源库介绍
RL库中集成了以下开源含碰撞检测功能的库:
- 1.bullet3:https://pybullet.org/wordpress/
- 2.FCL:https://github.com/flexible-collision-library/fcl
- 3.ODE:http://www.ode.org/
- 4.PQP:http://gamma.cs.unc.edu/SSV/
- 5.SOLID:http://www.dtecta.com/
2.RL碰撞检测算法使用
RL的碰撞检测算法的话是先建立机器人和工作场景的wrl模型,然后根据运动学赋予场景中的对应模型以姿态,实时判断是否有组件发生碰撞,其算法的本质实现 是依赖于开源的bullet、FCL等库,然后rl库中添加了衔接文件是的上述方法均可使用。
- 关于场景、机器人.wrl模型建立可以参考我的另一篇文档:Robotics Library 之机器人建模(VRML)、工作场景Scances建模(VRML)
- 下边分别介绍RL库通过.wrl进行碰撞检测的实现:
2.1 简单场景碰撞检测
- 简单场景指的是单一的场景、单帧场景或者可以看作是静止不动的模型场景
- 下边仅通过 bullet 库进行碰撞检测测试
#include <iostream>
#include <rl/sg/Body.h>
#include <rl/sg/Model.h>
#include <rl/sg/SimpleScene.h>#define RL_SG_BULLET 1 //此处添加宏定义,下边的if语句方可生效#ifdef RL_SG_BULLET
#include <rl/sg/bullet/Scene.h>
#endif // RL_SG_BULLET
#ifdef RL_SG_FCL
#include <rl/sg/fcl/Scene.h>
#endif // RL_SG_FCL
#ifdef RL_SG_ODE
#include <rl/sg/ode/Scene.h>
#endif // RL_SG_ODE
#ifdef RL_SG_PQP
#include <rl/sg/pqp/Scene.h>
#endif // RL_SG_PQP
#ifdef RL_SG_SOLID
#include <rl/sg/solid/Scene.h>
#endif // RL_SG_SOLID//int
//main(int argc, char** argv)
//{
// if (argc < 2)
// {
// std::cout << "Usage: rlSceneCollisionTest SCENEFILE [shouldCollide]" << std::endl;
// return EXIT_FAILURE;
// }
//
// bool shouldCollide = (argc > 2);
int main()
{bool shouldCollide = true;std::vector<rl::sg::SimpleScene*> scenes;std::vector<std::string> sceneNames;#ifdef RL_SG_BULLETscenes.push_back(new rl::sg::bullet::Scene);sceneNames.push_back("bullet");
#endif // RL_SG_BULLET
#ifdef RL_SG_FCLscenes.push_back(new rl::sg::fcl::Scene);sceneNames.push_back("fcl");
#endif // RL_SG_FCL
#ifdef RL_SG_ODEscenes.push_back(new rl::sg::ode::Scene);sceneNames.push_back("ode");
#endif // RL_SG_ODE
#ifdef RL_SG_PQPscenes.push_back(new rl::sg::pqp::Scene);sceneNames.push_back("pqp");
#endif // RL_SG_PQP
#ifdef RL_SG_SOLIDscenes.push_back(new rl::sg::solid::Scene);sceneNames.push_back("solid");
#endif // RL_SG_SOLIDfor (std::size_t i = 0; i < scenes.size(); ++i){std::cout << "Loading " << sceneNames[i] << " scene" << std::endl;scenes[i]->load("puma560/unimation-puma560_boxes.xml.xml");}std::cout << "Loading done." << std::endl;std::cout << "Loading done." << std::endl;for (int i = 0; i < scenes->getNumModels(); ++i){rl::sg::Model* mdl = scenes->getModel(i);std::cout << "model" << i << "'name" << mdl->getName() << " have" << mdl->getNumBodies() << " bodies" << std::endl;for (int j = 0; j < mdl->getNumBodies(); ++j){std::cout << " body" << j << "'name:" << mdl->getBody(j)->getName() << std::endl;}}int errorlevel = EXIT_SUCCESS;std::cout << "Now testing, expect isColliding()=" << shouldCollide << std::endl;if (scenes->isColliding() ^ shouldCollide)//异或{ std::cerr << "Error: " << sceneNames << " reports SimpleScene::isColliding()=" << scenes->isColliding() << std::endl;errorlevel = EXIT_FAILURE;}else {std::cout << "isColliding() = " << scenes->isColliding() << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(1)->getBody(0)) << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(0)->getBody(3)) << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(0)->getBody(2)) << std::endl;}std::cout << "Testing done." << std::endl;return 0;
}
如下可以看出:模型的body名字,还有就是场景中的模型之间的碰撞关系:
2.2 复杂场景碰撞检测
复杂场景指机器人连续运动过程中
#include <iostream>
#include <memory>
#include <random>
#include <stdexcept>
#include <boost/lexical_cast.hpp>#include <rl/mdl/Kinematic.h>
#include <rl/mdl/XmlFactory.h>
#include <rl/sg/Body.h>
#include <rl/sg/Model.h>
#include <rl/sg/SimpleScene.h>
#include <rl/sg/bullet/Scene.h>//bullet库用于碰撞检测// mdl/sg-only version
// mdl/sg-only version
rl::math::Vector is_ScaneColliding(rl::sg::SimpleScene* scene)
{::rl::sg::Model* robotModel = scene->getModel(0);::rl::sg::Model* EWallModel = scene->getModel(1);rl::math::Vector res(robotModel->getNumBodies());for (::std::size_t i = 0; i < robotModel->getNumBodies(); ++i){res[i] = scene->areColliding(robotModel->getBody(i), EWallModel->getBody(0));/*if (res[i]){return true;}*/}return res;
}int main()
{std::cout.precision(4);std::cerr.precision(4);std::string model_path = "./puma560/unimation-puma560.xml";std::string scane_path = "./puma560/unimation-puma560_boxes.xml";std::vector<rl::math::Real>angles{ -24,3,3,0,0,0 };读入模型rl::mdl::XmlFactory factory;std::shared_ptr<rl::mdl::Model> model(factory.create(model_path));rl::mdl::Kinematic* kinematics = dynamic_cast<rl::mdl::Kinematic*>(model.get());std::size_t dof = kinematics->getDof();rl::math::Vector q(kinematics->getDof());for (std::size_t i = 0; i < kinematics->getDof(); ++i){q(i) = angles[i] * rl::math::DEG2RAD;}std::cout << "Set joint angle [rad] " << q.transpose() << std::endl;kinematics->setPosition(q);kinematics->forwardPosition();rl::sg::SimpleScene* scenes = new rl::sg::bullet::Scene;std::string sceneNames = "bullet";//导入场景模型文件std::cout << "Loading " << sceneNames << " scene" << std::endl;scenes->load(scane_path);assert(scenes[i]->getModel(0)->getNumBodies() == kinematics->getBodies());for (std::size_t b = 0; b < kinematics->getBodies(); ++b){scenes->getModel(0)->getBody(b)->setFrame(kinematics->getFrame(b));}std::cout << "Loading done." << std::endl;std::cout << "Testing SimpleScene::isColliding() in " << sceneNames << ": ";//std::cout << "collides(scenes, kinematics) = "<<(collides(scenes, kinematics) ? "true" : "false") << std::endl;std::mt19937 randomGenerator(0);std::uniform_real_distribution<rl::math::Real> randomDistribution(-180 * rl::math::DEG2RAD, 180 * rl::math::DEG2RAD);rl::math::Vector a0(dof); //1rl::math::Vector a1(dof); //1rl::math::Vector a2(dof); //1rl::math::Vector a3(dof); //1rl::math::Vector a4(dof); //1rl::math::Vector a5(dof); //0rl::math::Vector a6(dof); //0rl::math::Vector a7(dof); //0rl::math::Vector a8(dof); //0rl::math::Vector a9(dof); //0rl::math::Vector a10(dof); //0rl::math::Vector a11(dof); //0rl::math::Vector a12(dof); //0rl::math::Vector a13(dof); //0//有碰撞a0 << -101, -97, 90, 0, 0, 0;a1 << 27, -158, 90, 0, 0, 0;a2 << -105, -184, 90, 0, 0, 0;a3 << 143, -146, 90, 0, 0, 0;a4 << 135, -223, 90, 0, 0, 0;//无碰撞a5 << 76, -169, 90, 0, 0, 0;a6 << 88, -64, 90, 0, 0, 0;a7 << -15, -50, 90, 0, 0, 0;a8 << 40, -119, 90, 10, 12, 0;a9 << 2, -39, 90, 0, 0, 0;a10 << 0, 0, 0, 0, 0, 0;a11 << 0, 0, 0, 0, 6, 0;a12 << -10, -25, 90, 0, 6, 0;a13 << -10, -25, 90, 0, 6, 0;std::vector<rl::math::Vector>angs{ a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13 };std::size_t j;for (j = 0; j < angs.size(); ++j){rl::math::Vector q(dof);/*for (std::size_t i = 0; i < dof; ++i){q(i) = randomDistribution(randomGenerator);}*/q = angs[j] * rl::math::DEG2RAD;kinematics->setPosition(q);kinematics->forwardPosition();bool result;for (std::size_t b = 0; b < kinematics->getBodies(); ++b){scenes->getModel(0)->getBody(b)->setFrame(kinematics->getFrame(b));}rl::math::Vector res = is_ScaneColliding(scenes);//std::cout << res.transpose() << std::endl;std::cout << j << " : q: "<<q.transpose()*rl::math::RAD2DEG<<" result = " << res.transpose() << std::endl;}return EXIT_SUCCESS;
}
通过上述检测结果可对比软件仿真结果,检测结果是正确的