Incorrect distance result when using high gjk_tolerance
Closed this issue · 2 comments
Hi, in this following example, distance result is totally wrong when two objects are very close to each other when using high gjk_tolerance>1e-6
under Ubuntu 22.04 with clang version 15.0.7.
#include <hpp/fcl/distance.h>
#include <hpp/fcl/shape/geometric_shapes.h>
int main() {
auto box1 = std::make_shared<hpp::fcl::Box>(0.363, 0.277, 0.363);
auto obj1 = std::make_shared<hpp::fcl::CollisionObject>(box1);
hpp::fcl::Transform3f trans1;
trans1.rotation() << 0.9999195703, 0.01021849413, 0.007512347878,
-0.01021878248, 0.9999477869, -3.329855922e-17, -0.007511955635,
-7.676704888e-05, 0.9999717819;
trans1.translation() << 1.503354966, -0.8531707835, 0.7700092418;
obj1->setTransform(trans1);
obj1->computeAABB();
auto box2 = std::make_shared<hpp::fcl::Box>(0.363, 0.363, 0.277);
auto obj2 = std::make_shared<hpp::fcl::CollisionObject>(box2);
hpp::fcl::Transform3f trans2;
trans2.rotation() << 0.9998670967, -0.01297708324, 0.009868343982,
0.01297771517, 0.9999157859, 5.255670031e-17, -0.009867512928,
0.0001280685574, 0.9999513067;
trans2.translation() << 1.48838308, -1.102020834, 0.4497073799;
obj2->setTransform(trans2);
obj2->computeAABB();
hpp::fcl::DistanceRequest request;
request.gjk_tolerance = 1e-6;
hpp::fcl::DistanceResult result;
hpp::fcl::distance(obj1.get(), obj2.get(), request, result);
std::cout << "Distance: " << result.min_distance << std::endl;
}
# request.gjk_tolerance = 1e-6;
Distance: 3.57511e-06
# request.gjk_tolerance = 1e-3;
Distance: -1.79769e+308
Hi @j1nkun,
Thanks a lot for your interest in this library!
I ran your code (both in Debug and Release) using the latest commit of the devel
branch and it works fine.
We are currently working on releasing the devel branch into the next major version of this library. This will fix a lot of bugs with the current version of the library which is based on the master
branch. The issue you are encountering is probably related to one of them.
Thanks! I forgot to mention I'm using 2.4.4 in ros humble. Looking forward to the next major release.