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();
|
|||
|
|
}
|