nesterov_gjk.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2022, INRIA
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 Willow Garage, Inc. 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 #define BOOST_TEST_MODULE FCL_NESTEROV_GJK
38 #include <boost/test/included/unit_test.hpp>
39 
40 #include <Eigen/Geometry>
43 #include <hpp/fcl/internal/tools.h>
44 
45 #include "utility.h"
46 
47 using hpp::fcl::Box;
48 using hpp::fcl::Capsule;
50 using hpp::fcl::Convex;
52 using hpp::fcl::FCL_REAL;
58 using hpp::fcl::Triangle;
59 using hpp::fcl::Vec3f;
62 using std::size_t;
63 
64 BOOST_AUTO_TEST_CASE(set_gjk_variant) {
65  GJKSolver solver;
66  GJK gjk(128, 1e-6);
67  MinkowskiDiff shape;
68 
69  // Checking defaults
70  BOOST_CHECK(solver.gjk_variant == GJKVariant::DefaultGJK);
71  BOOST_CHECK(gjk.gjk_variant == GJKVariant::DefaultGJK);
72  BOOST_CHECK(shape.normalize_support_direction == false);
73 
74  // Checking set
77 
78  BOOST_CHECK(solver.gjk_variant == GJKVariant::NesterovAcceleration);
79  BOOST_CHECK(gjk.gjk_variant == GJKVariant::NesterovAcceleration);
80 }
81 
82 BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) {
83  Ellipsoid ellipsoid = Ellipsoid(1, 1, 1);
84  Box box = Box(1, 1, 1);
85  Convex<Triangle> cvx;
86 
87  MinkowskiDiff mink_diff1;
88  mink_diff1.set(&ellipsoid, &ellipsoid);
89  BOOST_CHECK(mink_diff1.normalize_support_direction == false);
90 
91  MinkowskiDiff mink_diff2;
92  mink_diff2.set(&ellipsoid, &box);
93  BOOST_CHECK(mink_diff2.normalize_support_direction == false);
94 
95  MinkowskiDiff mink_diff3;
96  mink_diff3.set(&cvx, &cvx);
97  BOOST_CHECK(mink_diff3.normalize_support_direction == true);
98 }
99 
100 void test_nesterov_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
101  // Solvers
102  unsigned int max_iterations = 128;
103  FCL_REAL tolerance = 1e-6;
104  GJK gjk(max_iterations, tolerance);
105  GJK gjk_nesterov(max_iterations, tolerance);
107 
108  // Minkowski difference
109  MinkowskiDiff mink_diff;
110 
111  // Generate random transforms
112  size_t n = 1000;
113  FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.};
114  std::vector<Transform3f> transforms;
115  generateRandomTransforms(extents, transforms, n);
116  Transform3f identity = Transform3f::Identity();
117 
118  // Same init for both solvers
119  Vec3f init_guess = Vec3f(1, 0, 0);
120  support_func_guess_t init_support_guess;
121  init_support_guess.setZero();
122 
123  for (size_t i = 0; i < n; ++i) {
124  mink_diff.set(&shape0, &shape1, identity, transforms[i]);
125 
126  // Evaluate both solvers twice, make sure they give the same solution
127  GJK::Status res_gjk_1 =
128  gjk.evaluate(mink_diff, init_guess, init_support_guess);
129  Vec3f ray_gjk = gjk.ray;
130  GJK::Status res_gjk_2 =
131  gjk.evaluate(mink_diff, init_guess, init_support_guess);
132  BOOST_CHECK(res_gjk_1 == res_gjk_2);
133  EIGEN_VECTOR_IS_APPROX(ray_gjk, gjk.ray, 1e-8);
134 
135  GJK::Status res_nesterov_gjk_1 =
136  gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
137  Vec3f ray_nesterov = gjk_nesterov.ray;
138  GJK::Status res_nesterov_gjk_2 =
139  gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
140  BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2);
141  EIGEN_VECTOR_IS_APPROX(ray_nesterov, gjk_nesterov.ray, 1e-8);
142 
143  // Make sure GJK and Nesterov accelerated GJK find the same distance between
144  // the shapes
145  BOOST_CHECK(res_nesterov_gjk_1 == res_gjk_1);
146  BOOST_CHECK_SMALL(fabs(ray_gjk.norm() - ray_nesterov.norm()), 1e-4);
147 
148  // Make sure GJK and Nesterov accelerated GJK converges in a reasonable
149  // amount of iterations
150  BOOST_CHECK(gjk.getIterations() < max_iterations);
151  BOOST_CHECK(gjk_nesterov.getIterations() < max_iterations);
152  }
153 }
154 
155 BOOST_AUTO_TEST_CASE(ellipsoid_ellipsoid) {
156  Ellipsoid ellipsoid0 = Ellipsoid(0.3, 0.4, 0.5);
157  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
158 
159  test_nesterov_gjk(ellipsoid0, ellipsoid1);
160  test_nesterov_gjk(ellipsoid0, ellipsoid1);
161 }
162 
163 BOOST_AUTO_TEST_CASE(ellipsoid_capsule) {
164  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
165  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
166  Capsule capsule0 = Capsule(0.1, 0.3);
167  Capsule capsule1 = Capsule(1.1, 1.3);
168 
169  test_nesterov_gjk(ellipsoid0, capsule0);
170  test_nesterov_gjk(ellipsoid0, capsule1);
171  test_nesterov_gjk(ellipsoid1, capsule0);
172  test_nesterov_gjk(ellipsoid1, capsule1);
173 }
174 
175 BOOST_AUTO_TEST_CASE(ellipsoid_box) {
176  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
177  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
178  Box box0 = Box(0.1, 0.2, 0.3);
179  Box box1 = Box(1.1, 1.2, 1.3);
180 
181  test_nesterov_gjk(ellipsoid0, box0);
182  test_nesterov_gjk(ellipsoid0, box1);
183  test_nesterov_gjk(ellipsoid1, box0);
184  test_nesterov_gjk(ellipsoid1, box1);
185 }
186 
187 BOOST_AUTO_TEST_CASE(ellipsoid_mesh) {
188  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
189  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
192 
193  test_nesterov_gjk(ellipsoid0, cvx0);
194  test_nesterov_gjk(ellipsoid0, cvx1);
195  test_nesterov_gjk(ellipsoid1, cvx0);
196  test_nesterov_gjk(ellipsoid1, cvx1);
197 }
198 
199 BOOST_AUTO_TEST_CASE(capsule_mesh) {
200  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
201  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
204  Capsule capsule0 = Capsule(0.1, 0.3);
205  Capsule capsule1 = Capsule(1.1, 1.3);
206 
207  test_nesterov_gjk(capsule0, cvx0);
208  test_nesterov_gjk(capsule0, cvx1);
209  test_nesterov_gjk(capsule1, cvx0);
210  test_nesterov_gjk(capsule1, cvx1);
211 }
212 
213 BOOST_AUTO_TEST_CASE(capsule_capsule) {
214  Capsule capsule0 = Capsule(0.1, 0.3);
215  Capsule capsule1 = Capsule(1.1, 1.3);
216 
217  test_nesterov_gjk(capsule0, capsule0);
218  test_nesterov_gjk(capsule1, capsule1);
219  test_nesterov_gjk(capsule0, capsule1);
220 }
221 
223  Box box0 = Box(0.1, 0.2, 0.3);
224  Box box1 = Box(1.1, 1.2, 1.3);
225  test_nesterov_gjk(box0, box0);
226  test_nesterov_gjk(box0, box1);
227  test_nesterov_gjk(box1, box1);
228 }
229 
231  Box box0 = Box(0.1, 0.2, 0.3);
232  Box box1 = Box(1.1, 1.2, 1.3);
233  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
234  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
237 
238  test_nesterov_gjk(box0, cvx0);
239  test_nesterov_gjk(box0, cvx1);
240  test_nesterov_gjk(box1, cvx0);
241  test_nesterov_gjk(box1, cvx1);
242 }
243 
245  Ellipsoid ellipsoid0 = Ellipsoid(0.5, 0.4, 0.3);
246  Ellipsoid ellipsoid1 = Ellipsoid(1.5, 1.4, 1.3);
249 
250  test_nesterov_gjk(cvx0, cvx0);
251  test_nesterov_gjk(cvx0, cvx1);
252  test_nesterov_gjk(cvx1, cvx1);
253 }
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::details::GJK::evaluate
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
Definition: src/narrowphase/gjk.cpp:624
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:141
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::details::GJK::getIterations
size_t getIterations()
Get GJK number of iterations.
Definition: gjk.h:255
hpp::fcl::DefaultGJK
@ DefaultGJK
Definition: data_types.h:83
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Definition: src/narrowphase/gjk.cpp:505
utility.h
hpp::fcl::details::GJK::ray
Vec3f ray
Definition: gjk.h:168
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
narrowphase.h
EIGEN_VECTOR_IS_APPROX
#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)
Definition: utility.h:59
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
hpp::fcl::Convex
Convex polytope.
Definition: shape/convex.h:50
hpp::fcl::Ellipsoid
Ellipsoid centered at point zero.
Definition: shape/geometric_shapes.h:258
hpp::fcl::GJKSolver::gjk_variant
GJKVariant gjk_variant
Variant to use for the GJK algorithm.
Definition: narrowphase.h:505
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::details::GJK::gjk_variant
GJKVariant gjk_variant
Definition: gjk.h:169
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::constructPolytopeFromEllipsoid
Convex< Triangle > constructPolytopeFromEllipsoid(const Ellipsoid &ellipsoid)
We give an ellipsoid as input. The output is a 20 faces polytope which vertices belong to the origina...
Definition: utility.cpp:477
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: shape/geometric_shapes.h:333
hpp::fcl::generateRandomTransforms
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:224
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
test_nesterov_gjk
void test_nesterov_gjk(const ShapeBase &shape0, const ShapeBase &shape1)
Definition: nesterov_gjk.cpp:100
hpp::fcl::details::MinkowskiDiff::normalize_support_direction
bool normalize_support_direction
Wether or not to use the normalize heuristic in the GJK Nesterov acceleration. This setting is only a...
Definition: gjk.h:95
geometric_shapes.h
tools.h
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:96
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(set_gjk_variant)
Definition: nesterov_gjk.cpp:64
hpp::fcl::GJKVariant
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:83
hpp::fcl::NesterovAcceleration
@ NesterovAcceleration
Definition: data_types.h:83
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
gjk
Definition: doc/gjk.py:1


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14