How to set security_margin and find min_distance

[jessicaleu24]

Hi,

I'm tying to set security_margin and find min_distance.
But I'm not confident with the numbers I'm getting

This is my current code

    `// Add all possible collision pairs 
    collision_model.addAllCollisionPairs();

    // Create data required by the algorithms
    pinocchio::GeometryData collision_data(collision_model);

    // Update Geometry models
    pinocchio::updateGeometryPlacements(model, data, collision_model, collision_data);

    // Print out the placement of each collision geometry object
    std::cout << "\nCollision object placements:" << std::endl;
    for(pinocchio::GeomIndex geom_id = 0; geom_id < (pinocchio::GeomIndex)collision_model.ngeoms; ++geom_id){
    
    // !!!!!!!!!!!!!!!!!!!!!!!!!!! IS the following the right way to set it?   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
            collision_data.collisionRequests[geom_id].security_margin = 0.1;
    // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
           
            std::cout << geom_id << ": "
            << std::fixed << std::setprecision(2)
            << collision_data.oMg[geom_id].translation().transpose()
            << " , "<<collision_data.collisionRequests[geom_id].security_margin
            << std::endl;

    }
    
    // And test all the collision pairs
    computeCollisions(collision_model, collision_data);

    std::cout << "Num of collision pair: " <<collision_model.collisionPairs.size() << std::endl;
    
    // Print the status of all the collision pairs
    for(size_t k = 0; k < collision_model.collisionPairs.size(); ++k)
    {
            const CollisionPair & cp = collision_model.collisionPairs[k];
            const hpp::fcl::CollisionResult & cr = collision_data.collisionResults[k];
            const hpp::fcl::DistanceResult & cd = collision_data.distanceResults[k];
            
            std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
            std::cout << (cr.isCollision() ? "yes" : "no");
            
            // !!!!!!!!!!!!!!!!!!!!!!!!!!! IS the following the right way to read it?   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
            std::cout << ", min_distance: "  << (double) cd.min_distance << std::endl; 
           // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    }`

Let me know if I'm doing the right thing

[jcarpent]

The collision distance is not filled because you have used computeCollisions and not computeDistances. In any case, computeCollisions also provides a lower bound of the distance, which is often sufficient for motion planning.