Incorrect Sphere Collision Detection: Spheres handled as Points in tesseract_gjk_pair_detector.cpp

[JayanthSuresh]

During discrete collision detection while using Bullet physics engine with calculate distance set to false, spheres are incorrectly treated as points, leading to inaccurate collision results. The issue originates in the btComputeSupport function within tesseract_gjk_pair_detector.cpp, where the support vertex is calculated without considering the shape's margin. For btSphereShape, the margin is essentially the radius of the sphere, meaning the function tends to return the origin (0,0,0) instead of an accurate point on the sphere's surface.

The function should calculate the support vertex taking into account the sphere's radius, thereby returning a point on the actual surface of the sphere, not the origin.

To address this issue, the calculation of pInANoMargin and qInBNoMargin should be adjusted based on the shape type. If the shape type is SPHERE_SHAPE_PROXYTYPE, the function localGetSupportVertexNonVirtual should be used instead of localGetSupportVertexWithoutMarginNonVirtual. See proposed change below:

btVector3 pInANoMargin = (convexA->getShapeType() == SPHERE_SHAPE_PROXYTYPE) ? convexA->localGetSupportVertexNonVirtual(separatingAxisInA) : convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);

btVector3 qInBNoMargin = (convexB->getShapeType() == SPHERE_SHAPE_PROXYTYPE) ? convexB->localGetSupportVertexNonVirtual(separatingAxisInB) : convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);

[Levi-Armstrong]

Would you have time to add a unit test which shows it failing and possible a patch to fix the problem?

[Levi-Armstrong]

@JayanthSuresh So I started to investigate this by create a unit test disabling calculate_distance shown in this PR #954 but was unable to recreate the issue. I did find a bug in FCL's handling of the calculate_distance which the PR fixes. Can you take a look and see what might be different between your setup and the unit test?

[JayanthSuresh]

@Levi-Armstrong Thanks for taking a stab at it. I am on an older version and I ran into a few issues trying to upgrade. I want to test it with the latest version to ensure the issue still persists. If it does, I will create a unit test. Will get back to you in a couple of days.

[Levi-Armstrong]

If you have any questions upgrading to the latest let me know; I am happy help.

[JayanthSuresh]

This is my catkin config:

robot@robot:~/tesseract_ws$ catkin build tesseract_collision

Profile: default
Extending: [explicit] /opt/ros/noetic
Workspace: /home/robot/tesseract_ws

Build Space: [exists] /home/robot/tesseract_ws/build
Devel Space: [exists] /home/robot/tesseract_ws/devel
Install Space: [unused] /home/robot/tesseract_ws/install
Log Space: [exists] /home/robot/tesseract_ws/logs
Source Space: [exists] /home/robot/tesseract_ws/src
DESTDIR: [unused] None

Devel Space Layout: linked
Install Space Layout: None

Additional CMake Args: -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING_ALL=ON
Additional Make Args: None
Additional catkin Make Args: None
Internal Make Job Server: True
Cache Job Environments: False

Buildlisted Packages: None
Skiplisted Packages: None

Workspace configuration appears valid.

I get the following while build tesseract_collision

Errors << tesseract_collision:make /home/robot/tesseract_ws/logs/tesseract_collision/build.make.001.log
Error running '/usr/bin/clang-tidy-12': PLEASE submit a bug report to https://bugs.llvm.org/ and include the crash backtrace.
Stack dump:
0. Program arguments: /usr/bin/clang-tidy-12 /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_octomap_mesh_unit.cpp -- /usr/bin/c++ -DBOOST_ALL_NO_LIB -DBOOST_FILESYSTEM_DYN_LINK -DBOOST_GRAPH_DYN_LINK -DBOOST_REGEX_DYN_LINK -DBOOST_SERIALIZATION_DYN_LINK -DBOOST_SYSTEM_DYN_LINK -DBT_USE_DOUBLE_PRECISION -DBoost_USE_MULTITHREADED=ON -DBoost_USE_STATIC_LIBS=OFF -DBoost_USE_STATIC_RUNTIME=OFF -DTESSERACT_ASSIMP_USE_PBRMATERIAL=1 -DTESSERACT_SUPPORT_DIR="/home/robot/tesseract_ws/devel/share/tesseract_support" -I/home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include -I/home/robot/tesseract_ws/src/tesseract/tesseract_collision/test_suite/include -I/home/robot/tesseract_ws/src/tesseract/tesseract_collision/bullet/include -I/home/robot/tesseract_ws/src/tesseract/tesseract_collision/fcl/include -isystem /usr/include/eigen3 -isystem /home/robot/tesseract_ws/devel/include -isystem /opt/ros/noetic/include -isystem /usr/include/bullet -O3 -DNDEBUG -Wall -Wextra -Wconversion -Wsign-conversion -Wno-sign-compare -Wnon-virtual-dtor -mno-avx -fprofile-arcs -ftest-coverage -mfpmath=sse -msse -msse2 -msse3 -mssse3 -fopenmp -std=gnu++17 -o CMakeFiles/tesseract_collision_octomap_mesh_unit.dir/collision_octomap_mesh_unit.cpp.o -c /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_octomap_mesh_unit.cpp

1. parser at end of file
   Stack dump without symbol names (ensure you have llvm-symbolizer in your PATH or set the environment var LLVM_SYMBOLIZER_PATH to point to it):
   /usr/lib/x86_64-linux-gnu/libLLVM-12.so.1(_ZN4llvm3sys15PrintStackTraceERNS_11raw_ostreamEi+0x23)[0x7f080a400ef3]
   /usr/lib/x86_64-linux-gnu/libLLVM-12.so.1(_ZN4llvm3sys17RunSignalHandlersEv+0x50)[0x7f080a3ff210]
   /usr/lib/x86_64-linux-gnu/libLLVM-12.so.1(+0xbd955f)[0x7f080a40155f]
   /usr/lib/x86_64-linux-gnu/libpthread.so.0(+0x14420)[0x7f0811c36420]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang4Sema19LookupSpecialMemberEPNS_13CXXRecordDeclENS0_16CXXSpecialMemberEbbbbb+0xc2)[0x7f08102d1ee2]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang4Sema16LookupDestructorEPNS_13CXXRecordDeclE+0x17)[0x7f08102d3147]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(+0xf64519)[0x7f080ff42519]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(+0xf64b15)[0x7f080ff42b15]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(+0xf63ef5)[0x7f080ff41ef5]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang4Sema17emitDeferredDiagsEv+0x149)[0x7f080ff39a39]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang4Sema33ActOnEndOfTranslationUnitFragmentENS0_14TUFragmentKindE+0x20d)[0x7f080ff3977d]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang4Sema25ActOnEndOfTranslationUnitEv+0x13f)[0x7f080ff39dbf]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang6Parser17ParseTopLevelDeclERNS_9OpaquePtrINS_12DeclGroupRefEEEb+0x48c)[0x7f080f9c5c8c]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang8ParseASTERNS_4SemaEbb+0x2ed)[0x7f080f916e3d]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang14FrontendAction7ExecuteEv+0x48)[0x7f0810eac118]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang16CompilerInstance13ExecuteActionERNS_14FrontendActionE+0x8a1)[0x7f0810e39dd1]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang7tooling21FrontendActionFactory13runInvocationESt10shared_ptrINS_18CompilerInvocationEEPNS_11FileManagerES2_INS_22PCHContainerOperationsEEPNS_18DiagnosticConsumerE+0x1ad)[0x7f081101d8ed]
   /usr/bin/clang-tidy-12[0x87f646]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang7tooling14ToolInvocation13runInvocationEPKcPNS_6driver11CompilationESt10shared_ptrINS_18CompilerInvocationEES7_INS_22PCHContainerOperationsEE+0x11a)[0x7f081101d64a]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang7tooling14ToolInvocation3runEv+0x9d4)[0x7f081101ca74]
   /usr/lib/x86_64-linux-gnu/libclang-cpp.so.12(_ZN5clang7tooling9ClangTool3runEPNS0_10ToolActionE+0xba8)[0x7f081101eb28]
   /usr/bin/clang-tidy-12[0x87c377]
   /usr/bin/clang-tidy-12[0x45833d]
   /usr/lib/x86_64-linux-gnu/libc.so.6(_libc_start_main+0xf3)[0x7f080930c083]
   /usr/bin/clang-tidy-12[0x45612e]
   Segmentation fault
   make[2]: *** [test/CMakeFiles/tesseract_collision_octomap_mesh_unit.dir/build.make:63: test/CMakeFiles/tesseract_collision_octomap_mesh_unit.dir/collision_octomap_mesh_unit.cpp.o] Error 1
   make[1]: *** [CMakeFiles/Makefile2:2132: test/CMakeFiles/tesseract_collision_octomap_mesh_unit.dir/all] Error 2
   make[1]: *** Waiting for unfinished jobs....
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:84:8: error: constructor does not initialize these fields: normal [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   struct ContactResult
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:130:3: error: constructor does not initialize these fields: normal [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   ContactResult() = default;
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:155:7: error: constructor does not initialize these fields: data [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   class ContactResultMap
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:415:8: error: constructor does not initialize these fields: modify_object_enabled [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   struct ContactManagerConfig
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:469:8: error: constructor does not initialize these fields: state0, state1 [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   struct ContactTrajectorySubstepResults
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:496:8: error: constructor does not initialize these fields: substeps, state0, state1 [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   struct ContactTrajectoryStepResults
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/core/include/tesseract_collision/core/types.h:535:8: error: constructor does not initialize these fields: steps, joint_names [cppcoreguidelines-pro-type-member-init,-warnings-as-errors]
   struct ContactTrajectoryResults
   ^
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_core_unit.cpp:13:28: error: variable 'active_links' is not initialized [cppcoreguidelines-init-variables,-warnings-as-errors]
   std::vectorstd::string active_links{ "link_1", "link_2", "link_3" };
   ^
   = 0
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_core_unit.cpp:14:28: error: variable 'static_links' is not initialized [cppcoreguidelines-init-variables,-warnings-as-errors]
   std::vectorstd::string static_links{ "base_link", "part_link" };
   ^
   = 0
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_core_unit.cpp:16:51: error: variable 'check_pairs' is not initialized [cppcoreguidelines-init-variables,-warnings-as-errors]
   std::vector<tesseract_collision::ObjectPairKey> check_pairs;
   ^
   = 0
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_core_unit.cpp:27:51: error: variable 'pairs' is not initialized [cppcoreguidelines-init-variables,-warnings-as-errors]
   std::vector<tesseract_collision::ObjectPairKey> pairs =
   ^
   = 0
   /home/robot/tesseract_ws/src/tesseract/tesseract_collision/test/collision_core_unit.cpp:67:36: error: variable 'base_vertices' is not initialized [cppcoreguidelines-init-variables,-warnings-as-errors]
   tesseract_common::VectorVector3d base_vertices{};

[JayanthSuresh]

If I don't pass -DTESSERACT_ENABLE_TESTING_ALL=ON, it seems to build just fine. Tesseract [0.21.1]

[JayanthSuresh]

@Levi-Armstrong Looks like the Issue exists only in the older version. The PR #954 includes unit test that captures sphere collision checking, thanks for that!