test_fcl_sphere_box.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018. Toyota Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CNRS-LAAS and AIST nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <memory>
38 
39 #include <gtest/gtest.h>
40 #include <Eigen/Dense>
41 
42 #include "eigen_matrix_compare.h"
45 
46 // TODO(SeanCurtis-TRI): Modify this test so it can be re-used for the distance
47 // query -- such that the sphere is slightly separated instead of slightly
48 // penetrating. See test_sphere_box.cpp for an example.
49 
50 // This collides a box with a sphere. The box is long and skinny with size
51 // (w, d, h) and its geometric frame is aligned with the world frame.
52 // The sphere has radius r and is positioned at (sx, sy, sz) with an identity
53 // orientation. In this configuration, the sphere penetrates the box slightly
54 // on its face that faces in the +z direction. The contact is *fully* contained
55 // in that face. (As illustrated below.)
56 //
57 // Side view
58 // z small sphere
59 // ┆ ↓
60 // ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━◯━━━━━━┓ ┬
61 // ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄╂ x h
62 // ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴
63 // ┆
64 //
65 // ├────────────────── w ──────────────────┤
66 //
67 // Top view
68 // y small sphere
69 // ┆ ↓
70 // ┏━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┓ ┬
71 // ╂┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┼┄┄┄┄┄┄┄┄┄┄┄┄◯┄┄┄┄┄┄╂ x d
72 // ┗━━━━━━━━━━━━━━━━━━━┿━━━━━━━━━━━━━━━━━━━┛ ┴
73 // ┆
74 //
75 // Properties of expected outcome:
76 // - One contact *should* be reported,
77 // - Penetration depth δ should be: radius - (sz - h / 2),
78 // - Contact point should be at: [sx, sy, h / 2 - δ / 2], and
79 // - Contact normal should be [0, 0, -1] (pointing from sphere into box).
80 //
81 // NOTE: Orientation of the sphere should *not* make a difference and is not
82 // tested here; it relies on the sphere-box primitive algorithm unit tests to
83 // have robustly tested that.
84 //
85 // This test *fails* if GJK is used to evaluate the collision. It passes if the
86 // custom sphere-box algorithm is used, and, therefore, its purpose is to
87 // confirm that the custom algorithm is being applied. It doesn't exhaustively
88 // test sphere-box collision. It merely confirms the tested primitive algorithm
89 // has been wired up correctly.
90 template <typename S>
92  using fcl::Vector3;
93  using Real = typename fcl::constants<S>::Real;
94  const Real eps = fcl::constants<S>::eps();
95 
96  // Configure geometry.
97 
98  // Box and sphere dimensions.
99  const Real w = 0.115;
100  const Real h = 0.0025;
101  const Real d = 0.01;
102  // The magnitude of the box's extents along each axis.
103  const Vector3<S> half_size{w / 2, d / 2, h / 2};
104  const Real r = 0.0015;
105  auto sphere_geometry = std::make_shared<fcl::Sphere<S>>(r);
106  auto box_geometry = std::make_shared<fcl::Box<S>>(w, d, h);
107 
108  // Poses of the geometry.
110 
111  // Compute multiple sphere locations. All at the same height to produce a
112  // fixed, expected penetration depth of half of its radius. The reported
113  // position of the contact will have the x- and y- values of the sphere
114  // center, but be half the target_depth below the +z face, i.e.,
115  // pz = (h / 2) - (target_depth / 2)
116  const Real target_depth = r * 0.5;
117  // Sphere center's height (position in z).
118  const Real sz = h / 2 + r - target_depth;
119  const Real pz = h / 2 - target_depth / 2;
120  // This transform will get repeated modified in the nested for loop below.
122 
123  fcl::CollisionObject<S> sphere(sphere_geometry, X_WS);
124  fcl::CollisionObject<S> box(box_geometry, X_WB);
125 
126  // Expected results. (Expected position is defined inside the for loop as it
127  // changes with each new position of the sphere.)
128  const S expected_depth = target_depth;
129  // This normal direction assumes the *sphere* is the first collision object.
130  // If the order is reversed, the normal must be likewise reversed.
131  const Vector3<S> expected_normal = -Vector3<S>::UnitZ();
132 
133  // Set up the collision request.
134  fcl::CollisionRequest<S> collision_request(1 /* num contacts */,
135  true /* enable_contact */);
136  collision_request.gjk_solver_type = solver_type;
137 
138  // Sample around the surface of the +z face on the box for a fixed penetration
139  // depth. Note: the +z face extends in the +/- x and y directions up to the
140  // distance half_size. Notes on the selected samples:
141  // - We've picked positions such that the *whole* sphere projects onto the
142  // +z face (e.g., half_size(i) - r). This *guarantees* that the contact is
143  // completely contained in the +z face so there is no possible ambiguity
144  // in the results.
145  // - We've picked points out near the boundaries, in the middle, and *near*
146  // zero without being zero. The GJK algorithm can actually provide the
147  // correct result at the (eps, eps) sample points. We leave those sample
148  // points in to confirm no degradation.
149  const std::vector<Real> x_values{
150  -half_size(0) + r, -half_size(0) * S(0.5), -eps, 0, eps,
151  half_size(0) * S(0.5), half_size(0) - r};
152  const std::vector<Real> y_values{
153  -half_size(1) + r, -half_size(1) * S(0.5), -eps, 0, eps,
154  half_size(1) * S(0.5), half_size(1) - r};
155  for (Real sx : x_values) {
156  for (Real sy : y_values ) {
157  // Repose the sphere.
158  X_WS.translation() << sx, sy, sz;
159  sphere.setTransform(X_WS);
160 
161  auto evaluate_collision = [&collision_request, &X_WS](
163  S expected_depth, const Vector3<S>& expected_normal,
164  const Vector3<S>& expected_pos, Real eps) {
165  // Compute collision.
166  fcl::CollisionResult<S> collision_result;
167  std::size_t contact_count =
168  fcl::collide(s1, s2, collision_request, collision_result);
169 
170  // Test answers
171  if (contact_count == collision_request.num_max_contacts) {
172  std::vector<fcl::Contact<S>> contacts;
173  collision_result.getContacts(contacts);
174  GTEST_ASSERT_EQ(contacts.size(), collision_request.num_max_contacts);
175 
176  const fcl::Contact<S>& contact = contacts[0];
178  << "Sphere at: " << X_WS.translation().transpose();
181  eps,
183  << "Sphere at: " << X_WS.translation().transpose();
186  << "Sphere at: " << X_WS.translation().transpose();
187  } else {
188  EXPECT_TRUE(false) << "No contacts reported for sphere at: "
189  << X_WS.translation().transpose();
190  }
191  };
192 
193  Vector3<S> expected_pos{sx, sy, pz};
194  evaluate_collision(&sphere, &box, expected_depth, expected_normal,
195  expected_pos, eps);
196  evaluate_collision(&box, &sphere, expected_depth, -expected_normal,
197  expected_pos, eps);
198  }
199  }
200 }
201 
202 GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_ccd) {
203  LargeBoxSmallSphereTest<double>(fcl::GJKSolverType::GST_LIBCCD);
204  LargeBoxSmallSphereTest<float>(fcl::GJKSolverType::GST_LIBCCD);
205 }
206 
207 GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_indep) {
208  LargeBoxSmallSphereTest<double>(fcl::GJKSolverType::GST_INDEP);
209  LargeBoxSmallSphereTest<float>(fcl::GJKSolverType::GST_INDEP);
210 }
211 
212 //==============================================================================
213 int main(int argc, char* argv[]) {
214  ::testing::InitGoogleTest(&argc, argv);
215  return RUN_ALL_TESTS();
216 }
fcl::CollisionObject::setTransform
void setTransform(const Matrix3< S > &R, const Vector3< S > &T)
set object's transform
Definition: collision_object-inl.h:198
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::CollisionRequest::gjk_solver_type
GJKSolverType gjk_solver_type
Enumeration indicating the GJK solver implementation to use.
Definition: collision_request.h:78
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
collision_object.h
fcl::CollisionRequest::num_max_contacts
size_t num_max_contacts
The maximum number of contacts that can be returned.
Definition: collision_request.h:59
eigen_matrix_compare.h
fcl::CompareMatrices
::testing::AssertionResult CompareMatrices(const Eigen::MatrixBase< DerivedA > &m1, const Eigen::MatrixBase< DerivedB > &m2, double tolerance=0.0, MatrixCompareType compare_type=MatrixCompareType::absolute)
Definition: eigen_matrix_compare.h:63
fcl::constants::eps
static constexpr Real eps()
Returns ε for the precision of the underlying scalar type.
Definition: constants.h:151
fcl::Contact
Contact information returned by collision.
Definition: contact.h:48
fcl::Contact::normal
Vector3< S > normal
contact normal, pointing from o1 to o2
Definition: contact.h:71
expected_pos
Vector3< S > expected_pos
Definition: test_sphere_box.cpp:181
fcl::CollisionResult
collision result
Definition: collision_request.h:48
EXPECT_TRUE
#define EXPECT_TRUE(args)
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
distance.h
fcl::constants::Real
detail::ScalarTrait< S >::type Real
Definition: constants.h:131
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::Contact::pos
Vector3< S > pos
contact position, in world space
Definition: contact.h:74
fcl::MatrixCompareType::absolute
@ absolute
GTEST_TEST
GTEST_TEST(FCL_SPHERE_BOX, LargBoxSmallSphere_ccd)
Definition: test_fcl_sphere_box.cpp:202
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
expected_depth
S expected_depth
Definition: test_half_space_convex.cpp:152
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
r
S r
Definition: test_sphere_box.cpp:171
expected_normal
Vector3< S > expected_normal
Definition: test_sphere_box.cpp:180
fcl::CollisionResult::getContacts
void getContacts(std::vector< Contact< S >> &contacts_)
get all the contacts
Definition: collision_result-inl.h:107
half_size
Vector3< S > half_size
Definition: test_sphere_box.cpp:169
fcl::Contact::penetration_depth
S penetration_depth
penetration depth
Definition: contact.h:77
main
int main(int argc, char *argv[])
Definition: test_fcl_sphere_box.cpp:213
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
LargeBoxSmallSphereTest
void LargeBoxSmallSphereTest(fcl::GJKSolverType solver_type)
Definition: test_fcl_sphere_box.cpp:91


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49