Bullet collision detection (and distance computation) not working - Humble

[quizzy5889]

Description

I need to compute collision distance between obstacles and a custom robot. The obstacles are given as a set of solid primitives, more specifically boxes. If I use the default collision detector (FCL) collisions are correctly detected, but the distance seems not to be available.

For this I am trying to enable Bullet, but it seems not to work. First of all collisions are not correctly detected and the collision distance is always the same value.

Your environment

• ROS Distro: [Humble]
• OS Version: e.g. Ubuntu 22.04
• Source or Binary build? Source
• If binary, which release version? -
• If source, which branch? humble
• Which RMW (Fast DDS or Cyclone DDS)? Humble or Rolling should be using Cyclone. Fast DDS

Steps to reproduce

    planning_scene::PlanningScene planning_scene(kinematic_model)
    RCLCPP_INFO(this->get_logger(), "Loaded Scene");

    moveit_msgs::msg::CollisionObject collision_object = prepare_message(cloud);
    planning_scene.allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create());
    //planning_scene.allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
    planning_scene.processCollisionObjectMsg(collision_object);
    planning_scene.printKnownObjects();

    collision_detection::CollisionRequest collision_request;
    collision_request.contacts = true;
    collision_request.distance = true;

    collision_detection::CollisionResult collision_result;
    moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();
    collision_result.clear();

    planning_scene.checkCollision(collision_request, collision_result);
    RCLCPP_INFO(this->get_logger(), "Collisions computed");

    RCLCPP_INFO(this->get_logger(), "Current state is %s collision", collision_result.collision ? "in" : "not in");

    if (!collision_result.collision)
        RCLCPP_INFO(this->get_logger(), "Closest distance is %lf", collision_result.distance);

In prepare_message function a set of boxes are created as CollisionObjects starting from a given message

Expected behaviour

First, a collision should be detected. Take a look at this image. Rviz2 is showing that the robot is in collision and in fact FCL is able to detect it (look at the output in the terminal). Anyway the collision distance is not provided.

Actual behaviour

This is what happens if I use Bullet. Collision is not detected (look at the output in the terminal), not even speaking about the distance.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[henningkayser]

@quizzy5889 thanks for reporting this. Collision checking should definitely work with both plugins. I think Bullet had some lack of support for distance checking, but I haven't tested this recently. Could you provide a working code example (draft PR) that we can reproduce?

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[DiegoRomeres]

I’m also interested in this problem, particularly in how the collision distance will be exposed. Is there any plan to implement this feature in the near future?