gjk_convergence_criterion.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 #include "hpp/fcl/data_types.h"
38 #include <boost/test/tools/old/interface.hpp>
39 #define BOOST_TEST_MODULE FCL_NESTEROV_GJK
40 #include <boost/test/included/unit_test.hpp>
41 
42 #include <Eigen/Geometry>
45 #include <hpp/fcl/internal/tools.h>
46 
47 #include "utility.h"
48 
49 using hpp::fcl::Box;
50 using hpp::fcl::FCL_REAL;
57 using hpp::fcl::Vec3f;
60 using std::size_t;
61 
62 BOOST_AUTO_TEST_CASE(set_cv_criterion) {
63  GJKSolver solver;
64  GJK gjk(128, 1e-6);
65 
66  // Checking defaults
68  BOOST_CHECK(solver.gjk_convergence_criterion_type ==
70 
71  BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::VDB);
72  BOOST_CHECK(gjk.convergence_criterion_type ==
74 
75  // Checking set
77  gjk.convergence_criterion = GJKConvergenceCriterion::DualityGap;
79  gjk.convergence_criterion_type = GJKConvergenceCriterionType::Absolute;
80 
81  BOOST_CHECK(solver.gjk_convergence_criterion ==
83  BOOST_CHECK(solver.gjk_convergence_criterion_type ==
85 
86  BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::DualityGap);
87  BOOST_CHECK(gjk.convergence_criterion_type ==
89 }
90 
91 void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
92  const GJKConvergenceCriterionType cv_type) {
93  // Solvers
94  unsigned int max_iterations = 128;
95  // by default GJK uses the VDB convergence criterion, which is relative.
96  GJK gjk1(max_iterations, 1e-6);
97 
98  FCL_REAL tol;
99  switch (cv_type) {
100  // need to lower the tolerance when absolute
102  tol = 1e-8;
103  break;
105  tol = 1e-6;
106  break;
107  default:
108  throw std::logic_error("Wrong convergence criterion type");
109  }
110 
111  GJK gjk2(max_iterations, tol);
113  gjk2.convergence_criterion_type = cv_type;
114 
115  GJK gjk3(max_iterations, tol);
117  gjk3.convergence_criterion_type = cv_type;
118 
119  // Minkowski difference
120  MinkowskiDiff mink_diff;
121 
122  // Generate random transforms
123  size_t n = 1000;
124  FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.};
125  std::vector<Transform3f> transforms;
126  generateRandomTransforms(extents, transforms, n);
127  Transform3f identity = Transform3f::Identity();
128 
129  // Same init for both solvers
130  Vec3f init_guess = Vec3f(1, 0, 0);
131  support_func_guess_t init_support_guess;
132  init_support_guess.setZero();
133 
134  // Run for 3 different cv criterions
135  for (size_t i = 0; i < n; ++i) {
136  mink_diff.set(&shape0, &shape1, identity, transforms[i]);
137 
138  GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
139  BOOST_CHECK(gjk1.getIterations() <= max_iterations);
140  Vec3f ray1 = gjk1.ray;
141  res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
142  BOOST_CHECK(res1 != GJK::Status::Failed);
143  EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8);
144 
145  GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
146  BOOST_CHECK(gjk2.getIterations() <= max_iterations);
147  Vec3f ray2 = gjk2.ray;
148  res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
149  BOOST_CHECK(res2 != GJK::Status::Failed);
150  EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8);
151 
152  GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
153  BOOST_CHECK(gjk3.getIterations() <= max_iterations);
154  Vec3f ray3 = gjk3.ray;
155  res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
156  BOOST_CHECK(res3 != GJK::Status::Failed);
157  EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8);
158 
159  // check that solutions are close enough
160  EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk2.ray, 1e-4);
161  EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk3.ray, 1e-4);
162  }
163 }
164 
165 BOOST_AUTO_TEST_CASE(cv_criterion_same_solution) {
166  Box box0 = Box(0.1, 0.2, 0.3);
167  Box box1 = Box(1.1, 1.2, 1.3);
170 }
hpp::fcl::GJKSolver::gjk_convergence_criterion
GJKConvergenceCriterion gjk_convergence_criterion
Criterion used to stop GJK.
Definition: narrowphase.h:508
data_types.h
hpp::fcl::GJKConvergenceCriterionType
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:93
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
hpp::fcl::VDB
@ VDB
Definition: data_types.h:89
hpp::fcl::details::GJK::getIterations
size_t getIterations()
Get GJK number of iterations.
Definition: gjk.h:255
hpp::fcl::GJKConvergenceCriterion
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:89
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
hpp::fcl::Hybrid
@ Hybrid
Definition: data_types.h:89
narrowphase.h
test_gjk_cv_criterion
void test_gjk_cv_criterion(const ShapeBase &shape0, const ShapeBase &shape1, const GJKConvergenceCriterionType cv_type)
Definition: gjk_convergence_criterion.cpp:91
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::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::GJKSolver::gjk_convergence_criterion_type
GJKConvergenceCriterionType gjk_convergence_criterion_type
Relative or absolute.
Definition: narrowphase.h:511
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::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
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::Relative
@ Relative
Definition: data_types.h:93
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(set_cv_criterion)
Definition: gjk_convergence_criterion.cpp:62
hpp::fcl::details::GJK::convergence_criterion
GJKConvergenceCriterion convergence_criterion
Definition: gjk.h:170
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
geometric_shapes.h
hpp::fcl::details::GJK::convergence_criterion_type
GJKConvergenceCriterionType convergence_criterion_type
Definition: gjk.h:171
tools.h
hpp::fcl::DualityGap
@ DualityGap
Definition: data_types.h:89
hpp::fcl::Absolute
@ Absolute
Definition: data_types.h:93
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: shape/geometric_shapes.h:51
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:13