Incorrect Collision and Distance Results with Box and Capsule

[samarthjain96]

Hi, I am trying to model robot collision with static obstacles. The links of the robot have been modeled as Capsules and obstacle as primitive shapes. Sphere and Cylinder are working okay but when collision and distance check is done with Box, the results are incorrect. All shapes have their respective classes. Box is defined in below code:

Box box("Box", 0.3, 0.3, 0.5);
Eigen::Matrix4d box_transform_eigen;
box_transform_eigen.setIdentity();
box_transform_eigen.block<3,1>(0,3) = Eigen::Vector3d{0.6, 0, 0.25};
box.setTransformation(box_transform_eigen);

Collision check and distance check is as below.

bool Box::checkCollisionCapsule(fcl::CollisionObject<double> capsuleObject){

    fcl::CollisionObject<double> box_coll_obj = createBoxCollisionObject();

    fcl::CollisionRequest<double> collision_request;
    fcl::CollisionResult<double> collision_result;
    collision_request.gjk_solver_type = fcl::GST_INDEP; /// Needed for box

    fcl::collide(&box_coll_obj, &capsuleObject, collision_request, collision_result);

    return collision_result.isCollision();
}

double Box::getDistanceCapsule(fcl::CollisionObject<double> capsuleObject){

    fcl::DistanceRequestd distance_request;
    fcl::DistanceResultd distance_result;

    distance_request.gjk_solver_type = fcl::GST_INDEP; /// Needed for box

    fcl::CollisionObject<double> box_coll_obj = createBoxCollisionObject();

    fcl::distance(&box_coll_obj, &capsuleObject, distance_request, distance_result);   

    return distance_result.min_distance;
}

For robot link 2 and 3 I am getting collision even though they are not colliding as shown in figure.

Woul be be great if someone helps in this issue.

Thanks

[samarthjain96]

Gazebo reference frame was shifted , so the box was spawning in wrong position. It is fixed now. FCL is running fine with box and capsule.