230 lines
10 KiB
C++
230 lines
10 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 <sean@tri.global> (2018) */
|
||
|
||
#include <memory>
|
||
|
||
#include <gtest/gtest.h>
|
||
#include <Eigen/Dense>
|
||
|
||
#include "eigen_matrix_compare.h"
|
||
#include "fcl/narrowphase/collision_object.h"
|
||
#include "fcl/narrowphase/distance.h"
|
||
|
||
// TODO(SeanCurtis-TRI): Modify this test so it can be re-used for the distance
|
||
// query -- such that the sphere is slightly separated instead of slightly
|
||
// penetrating. See test_sphere_cylinder.cpp for an example.
|
||
|
||
// This collides a cylinder with a sphere. The cylinder is disk-like with a
|
||
// large radius (r_c) and small height (h_c) and its geometric frame is aligned
|
||
// with the world frame. The sphere has radius r_s and is positioned at
|
||
// (sx, sy, sz) with an identity orientation. In this configuration, the sphere
|
||
// penetrates the cylinder slightly on the top face near the edge. The contact
|
||
// is *fully* contained in that face. (As illustrated below.)
|
||
//
|
||
// Side view
|
||
// z small sphere
|
||
// ┆ ↓
|
||
// ┏━━━━━━━━━━━━┿━━━━◯━━━━━━┓ ┬
|
||
// ┄┄┄┄┄┄╂┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄╂┄ x h_c
|
||
// ┗━━━━━━━━━━━━┿━━━━━━━━━━━┛ ┴
|
||
// ┆
|
||
//
|
||
// ├──── r_c───┤
|
||
//
|
||
// Top view
|
||
// y
|
||
// ┆
|
||
// ******* small sphere ┬
|
||
// ** ┆ **╱ │
|
||
// * ┆ ◯ * │
|
||
// * ┆ * │
|
||
// * ┆ * r_c
|
||
// * ┆ * │
|
||
// * ┆ * │
|
||
// * ┆ * │
|
||
// ┄┄┄┄┄┄┄*┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄*┄┄┄┄ x ┴
|
||
// * ┆ *
|
||
// * ┆ *
|
||
// * ┆ *
|
||
// * ┆ *
|
||
// * ┆ *
|
||
// * ┆ *
|
||
// ** ┆ **
|
||
// *******
|
||
// ┆
|
||
// Properties of expected outcome:
|
||
// - One contact *should* be reported,
|
||
// - Penetration depth δ should be: r_s - (sz - h_c / 2),
|
||
// - Contact point should be at: [sx, sy, h_c / 2 - δ / 2], and
|
||
// - Contact normal should be [0, 0, -1] (pointing from sphere into cylinder).
|
||
//
|
||
// NOTE: Orientation of the sphere should *not* make a difference and is not
|
||
// tested here; it relies on the sphere-cylinder primitive algorithm unit tests
|
||
// to have robustly tested that.
|
||
//
|
||
// This test *fails* if GJK is used to evaluate the collision. It passes if the
|
||
// custom sphere-cylinder algorithm is used, and, therefore, its purpose is to
|
||
// confirm that the custom algorithm is being applied. It doesn't exhaustively
|
||
// test sphere-cylinder collision. It merely confirms the tested primitive
|
||
// algorithm has been wired up correctly.
|
||
template <typename S>
|
||
void LargeCylinderSmallSphereTest(fcl::GJKSolverType solver_type) {
|
||
using fcl::Vector3;
|
||
using Real = typename fcl::constants<S>::Real;
|
||
const Real eps = fcl::constants<S>::eps();
|
||
|
||
// Configure geometry.
|
||
|
||
// Cylinder and sphere dimensions.
|
||
const Real r_c = 9;
|
||
const Real h_c = 0.0025;
|
||
const Real r_s = 0.0015;
|
||
|
||
auto sphere_geometry = std::make_shared<fcl::Sphere<S>>(r_s);
|
||
auto cylinder_geometry = std::make_shared<fcl::Cylinder<S>>(r_c, h_c);
|
||
|
||
// Pose of the cylinder in the world frame.
|
||
const fcl::Transform3<S> X_WC = fcl::Transform3<S>::Identity();
|
||
|
||
// Compute multiple sphere locations. All at the same height to produce a
|
||
// fixed, expected penetration depth of half of its radius. The reported
|
||
// position of the contact will have the x- and y- values of the sphere
|
||
// center, but be half the target_depth below the +z face, i.e.,
|
||
// pz = (h_c / 2) - (target_depth / 2)
|
||
const Real target_depth = r_s * 0.5;
|
||
// Sphere center's height (position in z).
|
||
const Real sz = h_c / 2 + r_s - target_depth;
|
||
const Real pz = h_c / 2 - target_depth / 2;
|
||
// This transform will get repeatedly modified in the nested for loop below.
|
||
fcl::Transform3<S> X_WS = fcl::Transform3<S>::Identity();
|
||
|
||
fcl::CollisionObject<S> sphere(sphere_geometry, X_WS);
|
||
fcl::CollisionObject<S> cylinder(cylinder_geometry, X_WC);
|
||
|
||
// Expected results. (Expected position is defined inside the for loop as it
|
||
// changes with each new position of the sphere.)
|
||
const S expected_depth = target_depth;
|
||
// This normal direction assumes the *sphere* is the first collision object.
|
||
// If the order is reversed, the normal must be likewise reversed.
|
||
const Vector3<S> expected_normal = -Vector3<S>::UnitZ();
|
||
|
||
// Set up the collision request.
|
||
fcl::CollisionRequest<S> collision_request(1 /* num contacts */,
|
||
true /* enable_contact */);
|
||
collision_request.gjk_solver_type = solver_type;
|
||
|
||
// Sample around the surface of the +z face on the disk for a fixed
|
||
// penetration depth. Note: the +z face is a disk with radius r_c. Notes on
|
||
// the selected samples:
|
||
// - We've picked positions such that the *whole* sphere projects onto the
|
||
// +z face. This *guarantees* that the contact is completely contained in
|
||
// the +z face so there is no possible ambiguity in the results.
|
||
// - We've picked points out near the boundaries, in the middle, and *near*
|
||
// zero without being zero. The GJK algorithm can actually provide the
|
||
// correct result at the (eps, eps) sample points. We leave those sample
|
||
// points in to confirm no degradation.
|
||
const std::vector<Real> r_values{0, eps, r_c / 2, r_c - r_s};
|
||
const S pi = fcl::constants<S>::pi();
|
||
const std::vector<Real> theta_values{0, pi/2, pi, 3 * pi / 4};
|
||
|
||
for (const Real r : r_values) {
|
||
for (const Real theta : theta_values ) {
|
||
// Don't just evaluate at nice, axis-aligned directions. Pick some number
|
||
// that can't be perfectly represented.
|
||
const Real angle = theta + pi / 7;
|
||
const Real sx = cos(angle) * r;
|
||
const Real sy = sin(angle) * r;
|
||
// Repose the sphere.
|
||
X_WS.translation() << sx, sy, sz;
|
||
sphere.setTransform(X_WS);
|
||
|
||
auto evaluate_collision = [&collision_request, &X_WS](
|
||
const fcl::CollisionObject<S>* s1, const fcl::CollisionObject<S>* s2,
|
||
S expected_depth, const Vector3<S>& expected_normal,
|
||
const Vector3<S>& expected_pos, Real eps) {
|
||
// Compute collision.
|
||
fcl::CollisionResult<S> collision_result;
|
||
std::size_t contact_count =
|
||
fcl::collide(s1, s2, collision_request, collision_result);
|
||
|
||
// Test answers
|
||
if (contact_count == collision_request.num_max_contacts) {
|
||
std::vector<fcl::Contact<S>> contacts;
|
||
collision_result.getContacts(contacts);
|
||
GTEST_ASSERT_EQ(contacts.size(), collision_request.num_max_contacts);
|
||
|
||
const fcl::Contact<S>& contact = contacts[0];
|
||
EXPECT_NEAR(contact.penetration_depth, expected_depth, eps)
|
||
<< "Sphere at: " << X_WS.translation().transpose();
|
||
EXPECT_TRUE(fcl::CompareMatrices(contact.normal,
|
||
expected_normal,
|
||
eps,
|
||
fcl::MatrixCompareType::absolute))
|
||
<< "Sphere at: " << X_WS.translation().transpose();
|
||
EXPECT_TRUE(fcl::CompareMatrices(
|
||
contact.pos, expected_pos, eps, fcl::MatrixCompareType::absolute))
|
||
<< "Sphere at: " << X_WS.translation().transpose();
|
||
} else {
|
||
EXPECT_TRUE(false) << "No contacts reported for sphere at: "
|
||
<< X_WS.translation().transpose();
|
||
}
|
||
};
|
||
|
||
Vector3<S> expected_pos{sx, sy, pz};
|
||
evaluate_collision(&sphere, &cylinder, expected_depth, expected_normal,
|
||
expected_pos, eps);
|
||
evaluate_collision(&cylinder, &sphere, expected_depth, -expected_normal,
|
||
expected_pos, eps);
|
||
}
|
||
}
|
||
}
|
||
|
||
GTEST_TEST(FCL_SPHERE_CYLINDER, LargCylinderSmallSphere_ccd) {
|
||
LargeCylinderSmallSphereTest<double>(fcl::GJKSolverType::GST_LIBCCD);
|
||
LargeCylinderSmallSphereTest<float>(fcl::GJKSolverType::GST_LIBCCD);
|
||
}
|
||
|
||
GTEST_TEST(FCL_SPHERE_CYLINDER, LargBoxSmallSphere_indep) {
|
||
LargeCylinderSmallSphereTest<double>(fcl::GJKSolverType::GST_INDEP);
|
||
LargeCylinderSmallSphereTest<float>(fcl::GJKSolverType::GST_INDEP);
|
||
}
|
||
|
||
//==============================================================================
|
||
int main(int argc, char* argv[]) {
|
||
::testing::InitGoogleTest(&argc, argv);
|
||
return RUN_ALL_TESTS();
|
||
}
|