471 lines
20 KiB
C++
471 lines
20 KiB
C++
/*
|
||
* 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 <functional>
|
||
#include <map>
|
||
#include <memory>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
#include <gtest/gtest.h>
|
||
#include <Eigen/Dense>
|
||
|
||
#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 <typename S> struct BoxSpecification {
|
||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||
fcl::Vector3<S> size;
|
||
fcl::Transform3<S> 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 <typename S>
|
||
class BoxBoxTest {
|
||
public:
|
||
// Construct the test scenario with the given box specifications. Box 2
|
||
// must be a *cube* (all sides equal).
|
||
BoxBoxTest(const BoxSpecification<S>& box_spec_1,
|
||
const BoxSpecification<S>& 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<S>::pi();
|
||
|
||
// Initialize isomorphic rotations of box 2.
|
||
iso_poses_["top"] = Transform3<S>::Identity();
|
||
iso_poses_["bottom"] =
|
||
Transform3<S>{AngleAxis<S>(pi, Vector3<S>::UnitX())};
|
||
iso_poses_["back"] =
|
||
Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitX())};
|
||
iso_poses_["front"] =
|
||
Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::UnitX())};
|
||
iso_poses_["left"] =
|
||
Transform3<S>{AngleAxis<S>(pi / 2, Vector3<S>::UnitY())};
|
||
iso_poses_["right"] =
|
||
Transform3<S>{AngleAxis<S>(3 * pi / 2, Vector3<S>::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<S>& expected_normal, S expected_depth,
|
||
std::function<void(const fcl::Vector3<S> &, S, const std::string &)>
|
||
contact_pos_test,
|
||
const std::string& origin_name) {
|
||
fcl::Contact<S> 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<S>& pre_pose = reorient_pair.second;
|
||
|
||
BoxSpecification<S> 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<S>& box_spec_A,
|
||
const BoxSpecification<S>& box_spec_B, fcl::GJKSolverType solver_type,
|
||
S test_tolerance, const fcl::Contact<S>& expected_contact,
|
||
std::function<void(const fcl::Vector3<S>&, S, const std::string&)>
|
||
contact_pos_test,
|
||
const std::string& origin_name) {
|
||
using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
|
||
CollisionGeometryPtr_t box_geometry_A(new fcl::Box<S>(box_spec_A.size));
|
||
CollisionGeometryPtr_t box_geometry_B(new fcl::Box<S>(box_spec_B.size));
|
||
|
||
fcl::CollisionObject<S> box_A(box_geometry_A, box_spec_A.X_FB);
|
||
fcl::CollisionObject<S> box_B(box_geometry_B, box_spec_B.X_FB);
|
||
|
||
// Compute collision - single contact and enable contact.
|
||
fcl::CollisionRequest<S> collisionRequest(1, true);
|
||
collisionRequest.gjk_solver_type = solver_type;
|
||
fcl::CollisionResult<S> collisionResult;
|
||
fcl::collide(&box_A, &box_B, collisionRequest, collisionResult);
|
||
EXPECT_TRUE(collisionResult.isCollision()) << origin_name;
|
||
std::vector<fcl::Contact<S>> contacts;
|
||
collisionResult.getContacts(contacts);
|
||
GTEST_ASSERT_EQ(contacts.size(), 1u) << origin_name;
|
||
|
||
const fcl::Contact<S>& 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<S> box_spec_1_;
|
||
const BoxSpecification<S> box_spec_2_;
|
||
map<string, fcl::Transform3<S>, std::less<string>,
|
||
Eigen::aligned_allocator<std::pair<const string, fcl::Transform3<S>>>>
|
||
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 <typename S>
|
||
void test_collision_box_box_all_contacts(fcl::GJKSolverType solver_type,
|
||
S test_tolerance)
|
||
{
|
||
const S pi = fcl::constants<S>::pi();
|
||
const S size_1 = 1;
|
||
BoxSpecification<S> box_1{
|
||
fcl::Vector3<S>{size_1, size_1, size_1},
|
||
fcl::Transform3<S>{fcl::AngleAxis<S>(pi / 4, fcl::Vector3<S>::UnitY())}};
|
||
|
||
const S size_2 = 3;
|
||
BoxSpecification<S> box_2{fcl::Vector3<S>{size_2, size_2, size_2},
|
||
fcl::Transform3<S>{fcl::Translation3<S>(
|
||
fcl::Vector3<S>(0, 0, -size_2 / 2))}};
|
||
|
||
fcl::Vector3<S> expected_normal{0, 0, -1};
|
||
S expected_depth = size_1 * sqrt(2) / 2;
|
||
|
||
auto contact_pos_test = [size_1](const fcl::Vector3<S> &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<S> 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<double>(fcl::GJKSolverType::GST_LIBCCD,
|
||
1e-14);
|
||
}
|
||
|
||
GTEST_TEST(FCL_BOX_BOX, collision_box_box_all_contacts_indep)
|
||
{
|
||
test_collision_box_box_all_contacts<double>(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 <typename S>
|
||
void test_collision_box_box_cull_contacts(fcl::GJKSolverType solver_type,
|
||
S test_tolerance)
|
||
{
|
||
const S pi = fcl::constants<S>::pi();
|
||
const S size = 1;
|
||
const S tilt = pi / 8;
|
||
BoxSpecification<S> box_1{
|
||
fcl::Vector3<S>{size, size, size},
|
||
fcl::Transform3<S>{fcl::AngleAxis<S>(tilt, fcl::Vector3<S>::UnitX())}};
|
||
|
||
BoxSpecification<S> box_2{fcl::Vector3<S>{size, size, size},
|
||
fcl::Transform3<S>{fcl::AngleAxis<S>(
|
||
pi / 4, fcl::Vector3<S>::UnitZ())}};
|
||
box_2.X_FB.translation() << 0, 0, -0.75;
|
||
|
||
fcl::Vector3<S> 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<S> &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<S>::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<S> 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<double>(fcl::GJKSolverType::GST_LIBCCD,
|
||
1e-14);
|
||
}
|
||
|
||
GTEST_TEST(FCL_BOX_BOX, collision_box_box_cull_contacts_indep)
|
||
{
|
||
test_collision_box_box_cull_contacts<double>(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 <typename S>
|
||
void test_collision_box_box_edge_contact(fcl::GJKSolverType solver_type,
|
||
S test_tolerance) {
|
||
const S pi = fcl::constants<S>::pi();
|
||
const S size = 1;
|
||
BoxSpecification<S> box_1{
|
||
fcl::Vector3<S>{size, size, size},
|
||
fcl::Transform3<S>{fcl::AngleAxis<S>(-pi / 4, fcl::Vector3<S>::UnitY())} *
|
||
fcl::Transform3<S>{fcl::AngleAxis<S>(pi / 4, fcl::Vector3<S>::UnitZ())}};
|
||
const fcl::Vector3<S> 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<S> box_2{fcl::Vector3<S>{size, size, size},
|
||
fcl::Transform3<S>::Identity()};
|
||
|
||
auto contact_pos_test = [expected_depth, size, dir](
|
||
const fcl::Vector3<S> &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<S> expected_pos{dir * dist};
|
||
EXPECT_TRUE(expected_pos.isApprox(pos, tolerance)) << origin_name
|
||
<< "\n\texpected: " << expected_pos.transpose()
|
||
<< "\n\tpos: " << pos.transpose();
|
||
};
|
||
|
||
BoxBoxTest<S> 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<double>(fcl::GJKSolverType::GST_LIBCCD,
|
||
1e-14);
|
||
}
|
||
|
||
GTEST_TEST(FCL_BOX_BOX, collision_box_box_edge_contact_indep)
|
||
{
|
||
test_collision_box_box_edge_contact<double>(fcl::GJKSolverType::GST_INDEP,
|
||
1e-14);
|
||
}
|
||
|
||
//==============================================================================
|
||
int main(int argc, char* argv[])
|
||
{
|
||
::testing::InitGoogleTest(&argc, argv);
|
||
return RUN_ALL_TESTS();
|
||
}
|