/* * 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 Sean Curtis */ #include #include #include #include #include #include #include #include "fcl/math/constants.h" #include "fcl/narrowphase/collision.h" #include "fcl/narrowphase/collision_object.h" using std::map; using std::pair; using std::string; using std::vector; // Simple specification for defining a box collision object. Specifies the // dimensions and pose of the box in some frame F (X_FB). For an explanation // of the notation X_FB, see: // http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html template struct BoxSpecification { EIGEN_MAKE_ALIGNED_OPERATOR_NEW fcl::Vector3 size; fcl::Transform3 X_FB; }; // Class for executing and evaluating various box-box tests. // The class is initialized with two box specifications (size and pose). // The test performs a collision test between the two boxes in 12 permutations: // One axis is the order (box 1 vs box 2 and box 2 vs box 1). // The other axis is the orientation box 2. Given that box 2 must be a cube, it // can be oriented in six different configurations and still produced the same // answer. // The 12 permutations are the two orderings crossed with the six orientations. template class BoxBoxTest { public: // Construct the test scenario with the given box specifications. Box 2 // must be a *cube* (all sides equal). BoxBoxTest(const BoxSpecification& box_spec_1, const BoxSpecification& box_spec_2) : box_spec_1_(box_spec_1), box_spec_2_(box_spec_2) { using fcl::AngleAxis; using fcl::Transform3; using fcl::Vector3; // Confirm box 2 is a cube EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(1)); EXPECT_EQ(box_spec_2.size(0), box_spec_2.size(2)); const S pi = fcl::constants::pi(); // Initialize isomorphic rotations of box 2. iso_poses_["top"] = Transform3::Identity(); iso_poses_["bottom"] = Transform3{AngleAxis(pi, Vector3::UnitX())}; iso_poses_["back"] = Transform3{AngleAxis(pi / 2, Vector3::UnitX())}; iso_poses_["front"] = Transform3{AngleAxis(3 * pi / 2, Vector3::UnitX())}; iso_poses_["left"] = Transform3{AngleAxis(pi / 2, Vector3::UnitY())}; iso_poses_["right"] = Transform3{AngleAxis(3 * pi / 2, Vector3::UnitY())}; } // Runs the 12 tests for the two specified boxes. // // @param solver_type The solver type to use for computing collision. // @param test_tolerance The tolerance to which the collision contact // results will be compared to the results. // @param expected_normal The expected normal for the (1, 2) query order. // @param expected_depth The expected penetration depth. // @param contact_pos_test A function to evaluate the reported contact // position for validity; this should be written to // account for the possibility of the contact // position lying on some manifold (e.g., an edge, or // face). This function should invoke googletest // EXPECT_* methods. // @param origin_name A string which is appended to error message to // more easily parse failures and which test failed. void RunTests(fcl::GJKSolverType solver_type, S test_tolerance, const fcl::Vector3& expected_normal, S expected_depth, std::function &, S, const std::string &)> contact_pos_test, const std::string& origin_name) { fcl::Contact expected_contact; expected_contact.penetration_depth = expected_depth; for (const auto& reorient_pair : iso_poses_) { const std::string& top_face = reorient_pair.first; const fcl::Transform3& pre_pose = reorient_pair.second; BoxSpecification box_2_posed{ box_spec_2_.size, box_spec_2_.X_FB * pre_pose }; // Collide (1, 2) expected_contact.normal = expected_normal; RunSingleTest(box_spec_1_, box_2_posed, solver_type, test_tolerance, expected_contact, contact_pos_test, origin_name + " (1, 2) - " + top_face); // Collide (2, 1) expected_contact.normal = -expected_normal; RunSingleTest(box_2_posed, box_spec_1_, solver_type, test_tolerance, expected_contact, contact_pos_test, origin_name + " (2, 1) - " + top_face); } } private: // Performs a collision test between two boxes and tests the *single* contact // result against given expectations. // // @param box_spec_A A specification of the first box (treated as object // 1 in the query). // @param box_spec_B A specification of the second box (treated as object // 2 in the query). // @param solver_type The solver type to use for computing collision. // @param test_tolerance The tolerance to which the collision contact results // will be compared to the results. // @param expected_contact The expected contact details (only penetration depth // and normal are used). // @param contact_pos_test A function to evaluate the reported contact position // for validity; this should be written to account for // the possibility of the contact position lying on // some manifold (e.g., an edge, or face). This // function should invoke googletest EXPECT_* methods. // @param origin_name A string which is appended to error message to more // easily parse failures and which test failed. void RunSingleTest( const BoxSpecification& box_spec_A, const BoxSpecification& box_spec_B, fcl::GJKSolverType solver_type, S test_tolerance, const fcl::Contact& expected_contact, std::function&, S, const std::string&)> contact_pos_test, const std::string& origin_name) { using CollisionGeometryPtr_t = std::shared_ptr>; CollisionGeometryPtr_t box_geometry_A(new fcl::Box(box_spec_A.size)); CollisionGeometryPtr_t box_geometry_B(new fcl::Box(box_spec_B.size)); fcl::CollisionObject box_A(box_geometry_A, box_spec_A.X_FB); fcl::CollisionObject box_B(box_geometry_B, box_spec_B.X_FB); // Compute collision - single contact and enable contact. fcl::CollisionRequest collisionRequest(1, true); collisionRequest.gjk_solver_type = solver_type; fcl::CollisionResult collisionResult; fcl::collide(&box_A, &box_B, collisionRequest, collisionResult); EXPECT_TRUE(collisionResult.isCollision()) << origin_name; std::vector> contacts; collisionResult.getContacts(contacts); GTEST_ASSERT_EQ(contacts.size(), 1u) << origin_name; const fcl::Contact& contact = contacts[0]; EXPECT_NEAR(expected_contact.penetration_depth, contact.penetration_depth, test_tolerance) << origin_name; EXPECT_TRUE(expected_contact.normal.isApprox(contact.normal, test_tolerance)) << origin_name << ":\n\texpected: " << expected_contact.normal.transpose() << "\n\tcontact.normal: " << contact.normal.transpose(); contact_pos_test(contact.pos, test_tolerance, origin_name); } const BoxSpecification box_spec_1_; const BoxSpecification box_spec_2_; map, std::less, Eigen::aligned_allocator>>> iso_poses_; }; // This test exercises the case of face-something contact. In the language of // boxBox2() (in box_box-inl.h) it is for codes 1-6. // // More particularly it is designed to exercise the case where no contact points // need be culled. It assumes that boxBox2() is invoked with a maximum contact // count of 4. The collision produces a manifold that is a four-sided polygon. // // The test looks like this: // // z // │ // │ // ╱│╲ // ╱ │ ╲ Box1 // ──┲━━╱━━O━━╲━━┱─── x // ┃ ╲ │ ╱ ┃ // ┃ ╲ │ ╱ ┃ // ┃ ╲│╱ ┃ // ┃ │ ┃ Box2 // ┃ │ ┃ // ┗━━━━━┿━━━━━┛ // │ // // There are two boxes: // Box 1: A cube with side length of 1, centered on the world origin (O) and // rotated 45 degrees around the y-axis. // Box 2: A cube with side length of 3, moved in the negative z-direction such // that it's top face lies on the z = 0 plane. // // The penetration depth should be sqrt(2) / 2. // The normal should be parallel with the z-axis. We test both the collision of // 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z, // respectively. // The contact position, should lie on an edge parallel with the y-axis at x = 0 // and z = sqrt(2) / 4. template void test_collision_box_box_all_contacts(fcl::GJKSolverType solver_type, S test_tolerance) { const S pi = fcl::constants::pi(); const S size_1 = 1; BoxSpecification box_1{ fcl::Vector3{size_1, size_1, size_1}, fcl::Transform3{fcl::AngleAxis(pi / 4, fcl::Vector3::UnitY())}}; const S size_2 = 3; BoxSpecification box_2{fcl::Vector3{size_2, size_2, size_2}, fcl::Transform3{fcl::Translation3( fcl::Vector3(0, 0, -size_2 / 2))}}; fcl::Vector3 expected_normal{0, 0, -1}; S expected_depth = size_1 * sqrt(2) / 2; auto contact_pos_test = [size_1](const fcl::Vector3 &pos, S tolerance, const std::string& origin_name) { const double expected_pos_z = -size_1 * std::sqrt(2) / 4; EXPECT_NEAR(expected_pos_z, pos(2), tolerance) << origin_name; EXPECT_NEAR(0, pos(0), tolerance) << origin_name; EXPECT_LE(pos(1), 0.5) << origin_name; EXPECT_GE(pos(1), -0.5) << origin_name; }; BoxBoxTest tests(box_1, box_2); tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth, contact_pos_test, "test_colliion_box_box_all_contacts"); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_ccd) { test_collision_box_box_all_contacts(fcl::GJKSolverType::GST_LIBCCD, 1e-14); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_indep) { test_collision_box_box_all_contacts(fcl::GJKSolverType::GST_INDEP, 1e-12); } // This test exercises the case of face-something contact. In the language of // boxBox2() (in box_box-inl.h) it is for codes 1-6. // // In contrast with the previous test (test_collision_box_box_all_contacts), // the contact manifold is an eight-sided polygon and contacts will need to be // culled. // // The test looks like this: // // Top view Side view // // +y +z // │ ┆ // ╱│╲ __━━━━━┓ Box1 // ┏━━━╱━━┿━━╲━━━┓ ┏━━━━━━━ ┆ ┃ // ┣━╱━━━━┿━━━━╲━┫ ┃ ┆ ┃ // ╱ │ ╲ ┃ ┆ ┃ // ___╱_┃______│______┃_╲__ +x ┄┄┸┰┄┄┄┄┄┄─┼┄─┄─┄┸┰┄┄┄┄ +y // ╲ ┃ │ ┃ ╱ ┃ ┆ ┃ // ╲ │ ╱ ┌─┨ ┏━━━┿━━━┓ ┠──┐ // ┃ ╲ │ ╱ ┃ │ ┃ ┃ │ ┖━━┛ │ // ┗━━━●━━┿━━●━━━┛ │ ┗━━━┛ │ │ // ╲│╱ │ │ │ Box2 // │ │ │ │ // │ │ │ │ // └─────────┴─────────┘ // // // There are two boxes: // Box 1: A cube with side length of 1, centered on the world origin and // rotated θ = 22.5 degrees around the x-axis // Box 2: A cube with side length of 1, rotated 45 degrees around the z-axis // and centered on the point [0, 0, -0.75]. // // The penetration depth should be √2/2 * cos(π/4 - θ) - 0.25 ≊ 0.4032814. // The normal should be parallel with the z-axis. We test both the collision of // 1 with 2 and 2 with 1. In those cases, the normal would be -z and +z, // respectively. // The contact position, should lie alone the line segment illustrated // in the top view (indicated by the two points '●') at z = -0.25 - depth / 2. template void test_collision_box_box_cull_contacts(fcl::GJKSolverType solver_type, S test_tolerance) { const S pi = fcl::constants::pi(); const S size = 1; const S tilt = pi / 8; BoxSpecification box_1{ fcl::Vector3{size, size, size}, fcl::Transform3{fcl::AngleAxis(tilt, fcl::Vector3::UnitX())}}; BoxSpecification box_2{fcl::Vector3{size, size, size}, fcl::Transform3{fcl::AngleAxis( pi / 4, fcl::Vector3::UnitZ())}}; box_2.X_FB.translation() << 0, 0, -0.75; fcl::Vector3 expected_normal{0, 0, -1}; const S expected_depth{sqrt(2) / 2 * cos(pi / 4 - tilt) - 0.25}; auto contact_pos_test = [expected_depth, tilt, pi]( const fcl::Vector3 &pos, S tolerance, const std::string& origin_name) { // Edge is parallel to the x-axis at // z = expected_z = -0.25 - depth / 2 // y = expected_y = -√2/2 * sin(π/4 - θ) // x = lines in the range [-x_e, x_e] where // x_e = √2/2 + expected_y const S expected_z{-0.25 - expected_depth / 2}; const S expected_y{-sqrt(2) / 2 * sin(pi / 4 - tilt)}; const S expected_x{sqrt(2) / 2 + expected_y + std::numeric_limits::epsilon()}; EXPECT_NEAR(expected_z, pos(2), tolerance) << origin_name; EXPECT_NEAR(expected_y, pos(1), tolerance) << origin_name; EXPECT_GE(pos(0), -expected_x); EXPECT_LE(pos(0), expected_x); }; BoxBoxTest tests(box_1, box_2); tests.RunTests(solver_type, test_tolerance, expected_normal, expected_depth, contact_pos_test, "test_collision_box_box_cull_contacts"); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_ccd) { test_collision_box_box_cull_contacts(fcl::GJKSolverType::GST_LIBCCD, 1e-14); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_indep) { test_collision_box_box_cull_contacts(fcl::GJKSolverType::GST_INDEP, 1e-14); } // This test exercises the case where the contact is between edges (rather than // face-something as in previous tests). // // The test looks like this. // // ╱╲ // ╱ ╲ Box1 // +z ╱ 1 ╲ // ╱╲ ╲ // ┆ ╱ ╲ ╱ // ┆╱ ╲ ╱ // ┏━━━━━╲━━━━┓ ╲ ╱ // ┃ ┆ ╲ ┃ ╱ // ┃ ┆ ╲┃ ╱ // ┄┄┄┄┄┃┄┄┄┄┼┄┄┄┄┄┃╲╱┄┄┄┄┄┄┄┄ +x // ┃ ┆ ┃ // ┃ ┆ ┃ Box2 // ┗━━━━━━━━━━┛ // ┆ // // There are two boxes: // Box 1: A cube with side length of 1. It is rotated 45° around the world's // z-axis and then -45° around the world's y-axis. Given a target // penetration depth of 0.1, it's center is finally moved along the // vector <√2/2, √2/2> a distance of `1 * √2 - 0.1`. // Box 2: A cube with side length of 1. It is aligned with and centered on // the world frame. // template void test_collision_box_box_edge_contact(fcl::GJKSolverType solver_type, S test_tolerance) { const S pi = fcl::constants::pi(); const S size = 1; BoxSpecification box_1{ fcl::Vector3{size, size, size}, fcl::Transform3{fcl::AngleAxis(-pi / 4, fcl::Vector3::UnitY())} * fcl::Transform3{fcl::AngleAxis(pi / 4, fcl::Vector3::UnitZ())}}; const fcl::Vector3 dir{sqrt(2) / 2, 0, sqrt(2) / 2}; const S expected_depth = 0.1; box_1.X_FB.translation() = dir * (size * sqrt(2) - expected_depth); BoxSpecification box_2{fcl::Vector3{size, size, size}, fcl::Transform3::Identity()}; auto contact_pos_test = [expected_depth, size, dir]( const fcl::Vector3 &pos, S tolerance, const std::string& origin_name) { // The contact point should unambiguously be a single point. const S dist = size * sqrt(2) / 2 - expected_depth / 2; const fcl::Vector3 expected_pos{dir * dist}; EXPECT_TRUE(expected_pos.isApprox(pos, tolerance)) << origin_name << "\n\texpected: " << expected_pos.transpose() << "\n\tpos: " << pos.transpose(); }; BoxBoxTest tests(box_1, box_2); tests.RunTests(solver_type, test_tolerance, -dir, expected_depth, contact_pos_test, "test_collision_box_box_edge_contact"); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_ccd) { test_collision_box_box_edge_contact(fcl::GJKSolverType::GST_LIBCCD, 1e-14); } GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_indep) { test_collision_box_box_edge_contact(fcl::GJKSolverType::GST_INDEP, 1e-14); } //============================================================================== int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }