test_fcl_gjk.cpp 11.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
/*
 *  Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, CNRS-LAAS.
 *  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 Willow Garage, Inc. 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 Florent Lamiraux <florent@laas.fr> */

#define BOOST_TEST_MODULE FCL_GJK
#define BOOST_TEST_DYN_LINK

#include <time.h>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes.h>

47
48
49
50
51
52
53
using hpp::fcl::GJKSolver_indep;
using hpp::fcl::TriangleP;
using hpp::fcl::Vec3f;
using hpp::fcl::Quaternion3f;
using hpp::fcl::Transform3f;
using hpp::fcl::Matrix3f;
using hpp::fcl::FCL_REAL;
54

55
56
57
58
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> vector_t;
typedef Eigen::Matrix<FCL_REAL, 6, 1> vector6_t;
typedef Eigen::Matrix<FCL_REAL, 4, 1> vector4_t;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
59
60
61
62
63
64
65
66
67
68
69
70

struct Result
{
  bool collision;
  clock_t timeGjk;
  clock_t timeGte;
}; // struct benchmark

typedef std::vector <Result> Results_t;

BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
{
71
72
73
74
  Eigen::IOFormat numpy (Eigen::FullPrecision, Eigen::DontAlignCols, ", ",
                         ", ", "np.array ((", "))", "", "");
  Eigen::IOFormat tuple (Eigen::FullPrecision, Eigen::DontAlignCols, "",
                         ", ", "", "", "(", ")");
75
76
77
78
79
80
81
82
83
84
85
86
  std::size_t N = 10000;
  GJKSolver_indep solver;
  Transform3f tf1, tf2;
  Vec3f p1, p2, a1, a2;
  Matrix3f M;
  FCL_REAL distance (sqrt (-1));
  clock_t start, end;

  std::size_t nCol = 0, nDiff = 0;
  FCL_REAL eps = 1e-7;
  Results_t results (N);
  for (std::size_t i=0; i<N; ++i) {
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
    Vec3f P1_loc (Vec3f::Random ()), P2_loc (Vec3f::Random ()),
      P3_loc (Vec3f::Random ());
    Vec3f Q1_loc (Vec3f::Random ()), Q2_loc (Vec3f::Random ()),
      Q3_loc (Vec3f::Random ());
    if (i==0) {
      P1_loc = Vec3f (0.063996093749999997, -0.15320971679687501,
                      -0.42799999999999999);
      P2_loc = Vec3f (0.069105957031249998, -0.150722900390625,
                      -0.42999999999999999);
      P3_loc = Vec3f (0.063996093749999997, -0.15320971679687501,
                  -0.42999999999999999);
      Q1_loc = Vec3f (-25.655000000000001, -1.2858199462890625,
                      3.7249809570312502);
      Q2_loc = Vec3f (-10.926, -1.284259033203125, 3.7281499023437501);
      Q3_loc = Vec3f (-10.926, -1.2866180419921875, 3.72335400390625);
      Transform3f tf
        (Quaternion3f (-0.42437287410898855, -0.26862477561450587,
                       -0.46249645019513175, 0.73064726592483387),
         Vec3f (-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
      tf1 = tf;
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
    } else if (i==1) {
      P1_loc = Vec3f
        (-0.8027043342590332 , -0.30276307463645935 , -0.4372950792312622);
      P2_loc = Vec3f
        (-0.8027043342590332 , 0.30276307463645935  ,-0.4372950792312622);
      P3_loc = Vec3f
        (0.8027043342590332  , 0.30276307463645935  ,-0.4372950792312622);
      Q1_loc = Vec3f
        (-0.224713996052742   , -0.7417119741439819 , 0.19999997317790985);
      Q2_loc = Vec3f
        (-0.5247139930725098 , -0.7417119741439819 , 0.19999997317790985);
      Q3_loc = Vec3f
        (-0.224713996052742   , -0.7417119741439819 ,  0.09999997168779373);
      Matrix3f R; Vec3f T;
      R <<  0.9657787025454787,      0.09400415350535746,   0.24173273843919627,
        -0.06713698817647556,    0.9908494114820345,    -0.11709000206805695,
        -0.25052768814676646,    0.09685382227587608,  0.9632524147814993;

      T << -0.13491177905469953, -1,  0.6000449621843792;
      tf1.setRotation(R); tf1.setTranslation(T);
127
128
129
130
131
132
    } else {
      tf1 = Transform3f ();
    }

    TriangleP tri1 (P1_loc, P2_loc, P3_loc);
    TriangleP tri2 (Q1_loc, Q2_loc, Q3_loc);
133
    Vec3f normal;
134

135
    bool res;
136
    start = clock ();
137
    res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, p1, p2, normal);
138
139
140
    end = clock ();
    results [i].timeGjk = end - start;
    results [i].collision = !res;
141
    assert (res == (distance > 0));
142
    if (!res) {
143
144
145
146
147
      Vec3f c1, c2, normal2;
      ++nCol;
      // check that moving triangle 2 by the penetration depth in the
      // direction of the normal makes the triangles collision free.
      FCL_REAL penetration_depth (-distance);
148
      assert (penetration_depth >= 0);
149
150
151
152
      tf2.setTranslation ((penetration_depth + 10-4) * normal);
      res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, c1, c2,
                                  normal2);
      if (!res) {
153
154
155
156
157
158
        std::cerr << "P1 = " << P1_loc.format (tuple) << std::endl;
        std::cerr << "P2 = " << P2_loc.format (tuple) << std::endl;
        std::cerr << "P3 = " << P3_loc.format (tuple) << std::endl;
        std::cerr << "Q1 = " << Q1_loc.format (tuple) << std::endl;
        std::cerr << "Q2 = " << Q2_loc.format (tuple) << std::endl;
        std::cerr << "Q3 = " << Q3_loc.format (tuple) << std::endl;
159
160
        std::cerr << "p1 = " << c1.format (tuple) << std::endl;
        std::cerr << "p2 = " << c2.format (tuple) << std::endl;
161
162
163
        std::cerr << "tf1 = " << tf1.getTranslation ().format (tuple)
                  << " + " << tf1.getQuatRotation ().coeffs ().format (tuple)
                  << std::endl;
164
165
166
167
168
169
170
171
        std::cerr << "tf2 = " << tf2.getTranslation ().format (tuple)
                  << " + " << tf2.getQuatRotation ().coeffs ().format (tuple)
                  << std::endl;
        std::cerr << "normal = " << normal.format (tuple) << std::endl;
        abort ();
      }
      distance = 0;
      tf2.setIdentity ();
172
173
    }
    // Compute vectors between vertices
174
175
176
    Vec3f P1 (tf1.transform (P1_loc)), P2 (tf1.transform (P2_loc)),
      P3 (tf1.transform (P3_loc)), Q1 (tf2.transform (Q1_loc)),
      Q2 (tf2.transform (Q2_loc)), Q3 (tf2.transform (Q3_loc));
177
178
179
180
181
182
183
184
185
    Vec3f u1 (P2 - P1);
    Vec3f v1 (P3 - P1);
    Vec3f w1 (u1.cross (v1));
    Vec3f u2 (Q2 - Q1);
    Vec3f v2 (Q3 - Q1);
    Vec3f w2 (u2.cross (v2));
    BOOST_CHECK (w1.squaredNorm () > eps*eps);
    M.col (0) = u1; M.col (1) = v1; M.col (2) = w1;
    // Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1
186
    a1 = M.inverse () * (p1 - P1);
187
188
189
    BOOST_CHECK (w2.squaredNorm () > eps*eps);
    // Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2
    M.col (0) = u2; M.col (1) = v2; M.col (2) = w2;
190
    a2 = M.inverse () * (p2 - Q1);
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268

    // minimal distance and closest points can be considered as a constrained
    // optimisation problem:
    //
    // min f (a1[0],a1[1], a2[0],a2[1])
    //   g1 (a1[0],a1[1], a2[0],a2[1]) <=0
    //   ...
    //   g6 (a1[0],a1[1], a2[0],a2[1]) <=0
    // with
    //  f (a1[0],a1[1], a2[0],a2[1]) =
    //         1                                                           2
    //        --- dist (P1 + a1[0] u1 + a1[1] v1, Q1 + a2[0] u2 + a2[1] v2),
    //         2
    //  g1 (a1[0],a1[1], a2[0],a2[1]) = -a1[0]
    //  g2 (a1[0],a1[1], a2[0],a2[1]) = -a1[1]
    //  g3 (a1[0],a1[1], a2[0],a2[1]) =  a1[0] + a1[1] - 1
    //  g4 (a1[0],a1[1], a2[0],a2[1]) = -a2[0]
    //  g5 (a1[0],a1[1], a2[0],a2[1]) = -a2[1]
    //  g6 (a1[0],a1[1], a2[0],a2[1]) =  a2[0] + a2[1] - 1

    // Compute gradient of f
    vector4_t grad_f;
    grad_f [0] = -(p2 - p1).dot (u1);
    grad_f [1] = -(p2 - p1).dot (v1);
    grad_f [2] = (p2 - p1).dot (u2);
    grad_f [3] = (p2 - p1).dot (v2);
    vector6_t g;
    g [0] = -a1 [0];
    g [1] = -a1 [1];
    g [2] = a1 [0] + a1 [1] - 1;
    g [3] = -a2 [0];
    g [4] = -a2 [1];
    g [5] = a2 [0] + a2 [1] - 1;
    matrix_t grad_g (4, 6); grad_g.setZero ();
    grad_g (0,0) = -1.;
    grad_g (1,1) = -1;
    grad_g (0,2) = 1; grad_g (1,2) = 1;
    grad_g (2,3) = -1;
    grad_g (3,4) = -1;
    grad_g (2,5) = 1; grad_g (3,5) = 1;
    // Check that closest points are on triangles planes
    // Projection of [P1p1] on line normal to triangle 1 plane is equal to 0
    BOOST_CHECK (fabs (a1 [2]) < eps);
    // Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0
    BOOST_CHECK (fabs (a2 [2]) < eps);

    /* Check Karush–Kuhn–Tucker conditions
                    6
                   __
                   \
         -grad f = /_  c  grad g
                   i=1  i       i

         where c  >= 0, and
                i
         c  g  = 0 for i between 1 and 6
          i  i
    */

    matrix_t Mkkt (4, 6); matrix_t::Index col = 0;
    // Check that constraints are satisfied
    for (vector6_t::Index j=0; j<6; ++j) {
      BOOST_CHECK (g [j] <= eps);
      // if constraint is saturated, add gradient in matrix
      if (fabs (g [j]) <= eps) {
        Mkkt.col (col) = grad_g.col (j); ++col;
      }
    }
    if (col > 0) {
      Mkkt.conservativeResize (4, col);
      // Compute KKT coefficients ci by inverting
      // Mkkt.c = -grad_f
      Eigen::JacobiSVD <matrix_t> svd
        (Mkkt, Eigen::ComputeThinU | Eigen::ComputeThinV);
      vector_t c (svd.solve (-grad_f));
      for (vector_t::Index j=0; j < c.size (); ++j) {
        BOOST_CHECK (c [j] >= -eps);
      }
269
270
    }
  }
271
  std::cerr << "nCol / nTotal = " << nCol << " / " << N << std::endl;
272
273
  std::cerr << "nDiff = " << nDiff << std::endl;
  // statistics
274
275
  clock_t totalTimeGjkColl = 0;
  clock_t totalTimeGjkNoColl = 0;
276
277
278
279
280
281
282
  for (std::size_t i=0; i<N; ++i) {
    if (results [i].collision) {
      totalTimeGjkColl += results [i].timeGjk;
    } else {
      totalTimeGjkNoColl += results [i].timeGjk;
    }
  }
283
284
  std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl + totalTimeGjkColl
            << ", " << FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC*N) << "s" << std::endl;
285
  std::cerr << "-- Collisions -------------------------" << std::endl;
286
  std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", " << FCL_REAL(totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC*nCol) << "s" << std::endl;
287
  std::cerr << "-- No collisions -------------------------" << std::endl;
288
  std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", " << FCL_REAL(totalTimeGjkNoColl) / FCL_REAL(CLOCKS_PER_SEC*(N-nCol)) << "s" << std::endl;
289
}