FCL碰撞检测C++测试

最新推荐文章于 2024-05-12 22:01:08 发布

siml142857

最新推荐文章于 2024-05-12 22:01:08 发布

阅读量821

点赞数 1

文章标签： c++ 开发语言

本文链接：https://blog.csdn.net/u014428224/article/details/131024036

版权

1 简介

基本测试，包含碰撞，距离，连续碰撞

2 code demo


#include <cmath>
#include <limits>
#include "fcl/narrowphase/distance.h"
#include "fcl/narrowphase/collision.h"
#include "fcl/narrowphase/collision_object.h"
#include "fcl/narrowphase/continuous_collision.h"

#include <iostream>
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <vector>
#include <math.h>

using CollisionGeometryd_sptr = std::shared_ptr<fcl::CollisionGeometry<double>>;


void YawToQuaternion(float yaw, Eigen::Quaterniond& q ){
    q.x() = 0;
    q.y() = 0;
    q.z() = sin(yaw/2);
    q.w() = cos(yaw/2);
}


void Pose2dToFclTf( double x, double y, double yaw, fcl::Transform3<double>& tf  ){

  tf.translation() = fcl::Vector3<double>(x, y, 0);
  Eigen::Quaterniond q;
  YawToQuaternion( yaw,q );
  tf.linear() = q.toRotationMatrix();
}

void testBoxVar(CollisionGeometryd_sptr box1, CollisionGeometryd_sptr box2 ){
 
    fcl::Transform3<double> box1_tf0;
    fcl::Transform3<double> box2_tf0;
    Pose2dToFclTf(0,0,0,box1_tf0);
    Pose2dToFclTf(0,3,0,box2_tf0);
    
    //box1.setTransform(box1_tf0);
    //box2.setTransform(box2_tf0);
    fcl::CollisionObject<double> cl_box1(box1, box1_tf0);
    fcl::CollisionObject<double> cl_box2(box2, box2_tf0);
    
    fcl::CollisionRequest<double> request;
    fcl::CollisionResult<double> result;
    fcl::DistanceRequest<double> distanceRequest;
    fcl::DistanceResult<double> distanceResult;
    distanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;
    distanceRequest.distance_tolerance = 1e-8;

    fcl::collide(&cl_box1, &cl_box2, request, result);
    fcl::distance (&cl_box1, &cl_box2, distanceRequest, distanceResult);
    if (result.isCollision()) {
        std::cout << "collision !!!" << std::endl;
    } else {
        std::cout << "safe !!! d=" << distanceResult.min_distance<< std::endl;
    }
}

void testPosVar( double x1, double y1, double yaw1, double x2, double y2, double yaw2 ){
    CollisionGeometryd_sptr box1(new  fcl::Box<double>(1.0, 2.0, 0.1)); 
    CollisionGeometryd_sptr box2(new  fcl::Box<double>(1.0, 1.0, 0.1)); 
    
    fcl::Transform3<double> box1_tf0;
    fcl::Transform3<double> box2_tf0;
    Pose2dToFclTf(x1,y1,yaw1,box1_tf0);
    Pose2dToFclTf(x2,y2,yaw2,box2_tf0);
    fcl::CollisionObject<double> cl_box1(box1, box1_tf0);
    fcl::CollisionObject<double> cl_box2(box2, box2_tf0);
  
    fcl::CollisionRequest<double> request;
    fcl::CollisionResult<double> result;
    fcl::DistanceRequest<double> distanceRequest;
    fcl::DistanceResult<double> distanceResult;
    distanceRequest.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;
    distanceRequest.distance_tolerance = 1e-8;

    fcl::collide(&cl_box1, &cl_box2, request, result);
    fcl::distance (&cl_box1, &cl_box2, distanceRequest, distanceResult);
    if (result.isCollision()) {
        std::cout << "collision !!!" << std::endl;
    } else {
        std::cout << "safe !!! d=" << distanceResult.min_distance<< std::endl;
    }
}
void testContinuousCollision(){
    printf("TEST Continuous Collision  !!! \n");
    CollisionGeometryd_sptr box1(new  fcl::Box<double>(1.0, 2.0, 0.1)); 
    CollisionGeometryd_sptr box2(new  fcl::Box<double>(1.0, 1.0, 0.1)); 
    fcl::Transform3<double> box1_tf0;
    fcl::Transform3<double> box1_tf1;
    fcl::Transform3<double> box2_tf0;
    fcl::Transform3<double> box2_tf1;
    Pose2dToFclTf(0,0,0,box1_tf0);
    Pose2dToFclTf(0,3,0,box2_tf0);
    Pose2dToFclTf(0,3,0,box1_tf1);
    Pose2dToFclTf(0,0,0,box2_tf1);

    fcl::CollisionObject<double> cl_box1(box1, box1_tf0);
    fcl::CollisionObject<double> cl_box2(box2, box2_tf0);

    fcl::ContinuousCollisionRequest<double>  req;
    fcl::ContinuousCollisionResult<double>  res;
    // 检测连续碰撞
    //fcl::continuousCollide(&capsule,tf1_beg, tf1_end, &box,tf2_beg, tf2_end, r1, r2);
    fcl::continuousCollide(&cl_box1, box1_tf1, &cl_box2, box2_tf1, req, res);
    if (res.is_collide) {
        std::cout << "collision !!!" << std::endl;
    } else {
        std::cout << "safe !!!" << std::endl;
    }
}

int main(int argc, char* argv[])
{
  //test_distance_capsule_box1<double>(fcl::GJKSolverType::GST_INDEP, 1e-8);

  testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(1.0, 2.0, 0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(1.0, 2.0, 0.1)));
  testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(1.0, 3.0, 0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(1.0, 3.0, 0.1)));
  testBoxVar(CollisionGeometryd_sptr(new fcl::Box<double>(2,   0,   0.1)),CollisionGeometryd_sptr( new fcl::Box<double>(2,     0, 0.1)));

  testPosVar(0,0,1.57, 0,1.5,0);
  testPosVar(0,0,0, 0,1.5,0);
  testPosVar(0,0,0, 0,1.51,0);

  testContinuousCollision();

}

cmake_minimum_required(VERSION 2.8)
project(fcl_test)
add_compile_options(-std=c++11)
find_package(FCL REQUIRED)
find_package(CCD REQUIRED)
include_directories(${FCL_INCLUDE_DIRS})
include_directories(${CCD_INCLUDE_DIRS})

add_executable(fcl1 ./fcl.cpp)
target_link_libraries(fcl1 ${FCL_LIBRARIES} ${CCD_LIBRARIES})
``