/* * Software License Agreement (BSD License) * * Copyright (c) 2018. Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of CNRS-LAAS and AIST nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** @author Hongkai Dai */ #include #include #include #include #include #include "eigen_matrix_compare.h" #include "fcl/narrowphase/collision_object.h" #include "fcl/narrowphase/distance.h" #include "fcl/narrowphase/distance_request.h" #include "fcl/narrowphase/distance_result.h" // For two spheres 1, 2, sphere 1 has radius1, and is centered at point A, with // coordinate p_FA in some frame F; sphere 2 has radius2, and is centered at // point B, with coordinate p_FB in the same frame F. Compute the (optionally // signed) distance between the two spheres. template S ComputeSphereSphereDistance(S radius1, S radius2, const fcl::Vector3& p_FA, const fcl::Vector3& p_FB, fcl::GJKSolverType solver_type, bool enable_nearest_points, bool enable_signed_distance, fcl::DistanceResult* result) { // Pose of the sphere expressed in the frame F. X_FA is the pose of sphere 1 // in frame F, while X_FB is the pose of sphere 2 in frame F. // We use monogram notation as in Drake, explained in // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html fcl::Transform3 X_FA, X_FB; X_FA.setIdentity(); X_FB.setIdentity(); X_FA.translation() = p_FA; X_FB.translation() = p_FB; fcl::DistanceRequest request; request.enable_nearest_points = enable_nearest_points; request.enable_signed_distance = enable_signed_distance; request.gjk_solver_type = solver_type; using CollisionGeometryPtr_t = std::shared_ptr>; CollisionGeometryPtr_t sphere_geometry_1(new fcl::Sphere(radius1)); CollisionGeometryPtr_t sphere_geometry_2(new fcl::Sphere(radius2)); fcl::CollisionObject sphere_1(sphere_geometry_1, X_FA); fcl::CollisionObject sphere_2(sphere_geometry_2, X_FB); const S min_distance = fcl::distance(&sphere_1, &sphere_2, request, *result); return min_distance; } template struct SphereSpecification { // @param p_FC_ the center of the sphere C measured and expressed in a frame F SphereSpecification(S m_radius, const fcl::Vector3& p_FC_) : radius(m_radius), p_FC(p_FC_) {} S radius; fcl::Vector3 p_FC; }; template void CheckSphereToSphereDistance(const SphereSpecification& sphere1, const SphereSpecification& sphere2, fcl::GJKSolverType solver_type, bool enable_nearest_points, bool enable_signed_distance, S min_distance_expected, const fcl::Vector3& p1_expected, const fcl::Vector3& p2_expected, S tol) { fcl::DistanceResult result; const S min_distance = ComputeSphereSphereDistance( sphere1.radius, sphere2.radius, sphere1.p_FC, sphere2.p_FC, solver_type, enable_nearest_points, enable_signed_distance, &result); EXPECT_NEAR(min_distance, min_distance_expected, tol); EXPECT_NEAR(result.min_distance, min_distance_expected, tol); EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[0], p1_expected, tol)); EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[1], p2_expected, tol)); } template struct SphereSphereDistance { SphereSphereDistance(const SphereSpecification& m_sphere1, const SphereSpecification& m_sphere2) : sphere1(m_sphere1), sphere2(m_sphere2) { min_distance = (sphere1.p_FC - sphere2.p_FC).norm() - sphere1.radius - sphere2.radius; const fcl::Vector3 AB = (sphere1.p_FC - sphere2.p_FC).normalized(); p_WP1 = sphere1.p_FC + AB * -sphere1.radius; p_WP2 = sphere2.p_FC + AB * sphere2.radius; } SphereSpecification sphere1; SphereSpecification sphere2; S min_distance; // The closest point P1 on sphere 1 is expressed in the world frame W. fcl::Vector3 p_WP1; // The closest point P2 on sphere 2 is expressed in the world frame W. fcl::Vector3 p_WP2; }; template void TestSeparatingSpheres(S tol, fcl::GJKSolverType solver_type) { std::vector> spheres; spheres.emplace_back(0.5, fcl::Vector3(0, 0, -1)); // sphere 1 spheres.emplace_back(0.6, fcl::Vector3(1.2, 0, 0)); // sphere 2 spheres.emplace_back(0.4, fcl::Vector3(-0.3, 0, 0)); // sphere 3 spheres.emplace_back(0.3, fcl::Vector3(0, 0, 1)); // sphere 4 for (int i = 0; i < static_cast(spheres.size()); ++i) { for (int j = i + 1; j < static_cast(spheres.size()); ++j) { SphereSphereDistance sphere_sphere_distance(spheres[i], spheres[j]); bool enable_signed_distance = true; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, sphere_sphere_distance.p_WP2, tol); // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, sphere_sphere_distance.p_WP1, tol); enable_signed_distance = false; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, sphere_sphere_distance.p_WP2, tol); // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, sphere_sphere_distance.p_WP1, tol); // TODO (hongkai.dai@tri.global): Test enable_nearest_points=false. } } } GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres_INDEP) { TestSeparatingSpheres(1E-14, fcl::GJKSolverType::GST_INDEP); } GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres_LIBCCD) { // TODO(hongkai.dai@tri.global): The accuracy of the closest point is only up // to 1E-3, although gjkSolver::distance_tolerance is 1E-6. We should // investigate the accuracy issue. // Specifically, when setting `enable_signed_distance = true`, then except for // the the pair of spheres (2, 3), the closest point between all other pairs // fail to achieve tolerance 1E-6. When `enable_signed_distance = false`, then // all pairs achieve tolerance 1E-6. TestSeparatingSpheres(1E-3, fcl::GJKSolverType::GST_LIBCCD); } //============================================================================== int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }