test/capsule_capsule.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE
39 #include <boost/test/included/unit_test.hpp>
40 
41 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
42 
43 #include <cmath>
44 #include <iostream>
45 #include <hpp/fcl/distance.h>
46 #include <hpp/fcl/collision.h>
47 #include <hpp/fcl/math/transform.h>
48 #include <hpp/fcl/collision.h>
49 #include <hpp/fcl/collision_object.h>
50 #include <hpp/fcl/shape/geometric_shapes.h>
51 
52 #include "utility.h"
53 
54 using namespace hpp::fcl;
55 
56 BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) {
57  const double radius = 1.;
58 
59  CollisionGeometryPtr_t c1(new Capsule(radius, 0.));
60  CollisionGeometryPtr_t c2(new Capsule(radius, 0.));
61 
62  CollisionGeometryPtr_t s1(new Sphere(radius));
63  CollisionGeometryPtr_t s2(new Sphere(radius));
64 
65 #ifndef NDEBUG
66  int num_tests = 1e3;
67 #else
68  int num_tests = 1e6;
69 #endif
70 
71  Transform3f tf1;
72  Transform3f tf2;
73 
74  for (int i = 0; i < num_tests; ++i) {
75  Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius);
76  Eigen::Vector3d p2 = Eigen::Vector3d::Random() * (2. * radius);
77 
78  Eigen::Matrix3d rot1 =
79  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
80  .toRotationMatrix();
81  Eigen::Matrix3d rot2 =
82  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
83  .toRotationMatrix();
84 
85  tf1.setTranslation(p1);
86  tf1.setRotation(rot1);
87  tf2.setTranslation(p2);
88  tf2.setRotation(rot2);
89 
90  CollisionObject capsule_o1(c1, tf1);
91  CollisionObject capsule_o2(c2, tf2);
92 
93  CollisionObject sphere_o1(s1, tf1);
94  CollisionObject sphere_o2(s2, tf2);
95 
96  // Enable computation of nearest points
97  CollisionRequest collisionRequest;
98  CollisionResult capsule_collisionResult, sphere_collisionResult;
99 
100  size_t sphere_num_collisions = collide(
101  &sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult);
102  size_t capsule_num_collisions = collide(
103  &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
104 
105  BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions);
106  if (sphere_num_collisions == 0 && capsule_num_collisions == 0)
107  BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound,
108  capsule_collisionResult.distance_lower_bound, 1e-6);
109  }
110 }
111 
112 BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) {
113  const double radius = 0.01;
114  const double length = 0.2;
115 
116  CollisionGeometryPtr_t c1(new Capsule(radius, length));
117  CollisionGeometryPtr_t c2(new Capsule(radius, length));
118 #ifndef NDEBUG
119  int num_tests = 1e3;
120 #else
121  int num_tests = 1e6;
122 #endif
123 
124  Transform3f tf1;
125  Transform3f tf2;
126 
127  Eigen::Vector3d p1 = Eigen::Vector3d::Zero();
128  Eigen::Vector3d p2_no_collision =
129  Eigen::Vector3d(0., 0.,
130  2 * (length / 2. + radius) +
131  1e-3); // because capsule are along the Z axis
132 
133  for (int i = 0; i < num_tests; ++i) {
134  Eigen::Matrix3d rot =
135  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
136  .toRotationMatrix();
137 
138  tf1.setTranslation(p1);
139  tf1.setRotation(rot);
140  tf2.setTranslation(p2_no_collision);
141  tf2.setRotation(rot);
142 
143  CollisionObject capsule_o1(c1, tf1);
144  CollisionObject capsule_o2(c2, tf2);
145 
146  // Enable computation of nearest points
147  CollisionRequest collisionRequest;
148  CollisionResult capsule_collisionResult;
149 
150  size_t capsule_num_collisions = collide(
151  &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
152 
153  BOOST_CHECK(capsule_num_collisions == 0);
154  }
155 
156  Eigen::Vector3d p2_with_collision =
157  Eigen::Vector3d(0., 0., std::min(length / 2., radius) * (1. - 1e-2));
158  for (int i = 0; i < num_tests; ++i) {
159  Eigen::Matrix3d rot =
160  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
161  .toRotationMatrix();
162 
163  tf1.setTranslation(p1);
164  tf1.setRotation(rot);
165  tf2.setTranslation(p2_with_collision);
166  tf2.setRotation(rot);
167 
168  CollisionObject capsule_o1(c1, tf1);
169  CollisionObject capsule_o2(c2, tf2);
170 
171  // Enable computation of nearest points
172  CollisionRequest collisionRequest;
173  CollisionResult capsule_collisionResult;
174 
175  size_t capsule_num_collisions = collide(
176  &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
177 
178  BOOST_CHECK(capsule_num_collisions > 0);
179  }
180 
181  p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3);
182 
183  Transform3f geom1_placement(Eigen::Matrix3d::Identity(),
184  Eigen::Vector3d::Zero());
185  Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision);
186 
187  for (int i = 0; i < num_tests; ++i) {
188  Eigen::Matrix3d rot =
189  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
190  .toRotationMatrix();
191  Eigen::Vector3d trans = Eigen::Vector3d::Random();
192 
193  Transform3f displacement(rot, trans);
194  Transform3f tf1 = displacement * geom1_placement;
195  Transform3f tf2 = displacement * geom2_placement;
196 
197  CollisionObject capsule_o1(c1, tf1);
198  CollisionObject capsule_o2(c2, tf2);
199 
200  // Enable computation of nearest points
201  CollisionRequest collisionRequest;
202  CollisionResult capsule_collisionResult;
203 
204  size_t capsule_num_collisions = collide(
205  &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
206 
207  BOOST_CHECK(capsule_num_collisions == 0);
208  }
209 
210  // p2_with_collision =
211  // Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2));
212  p2_with_collision = Eigen::Vector3d(0., 0., 0.01);
213  geom2_placement.setTranslation(p2_with_collision);
214 
215  for (int i = 0; i < num_tests; ++i) {
216  Eigen::Matrix3d rot =
217  Eigen::Quaterniond(Eigen::Vector4d::Random().normalized())
218  .toRotationMatrix();
219  Eigen::Vector3d trans = Eigen::Vector3d::Random();
220 
221  Transform3f displacement(rot, trans);
222  Transform3f tf1 = displacement * geom1_placement;
223  Transform3f tf2 = displacement * geom2_placement;
224 
225  CollisionObject capsule_o1(c1, tf1);
226  CollisionObject capsule_o2(c2, tf2);
227 
228  // Enable computation of nearest points
229  CollisionRequest collisionRequest;
230  CollisionResult capsule_collisionResult;
231 
232  size_t capsule_num_collisions = collide(
233  &capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
234 
235  BOOST_CHECK(capsule_num_collisions > 0);
236  }
237 }
238 
239 BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) {
240  CollisionGeometryPtr_t s1(new Capsule(5, 10));
241  CollisionGeometryPtr_t s2(new Capsule(5, 10));
242 
243  Transform3f tf1;
244  Transform3f tf2(Vec3f(20.1, 0, 0));
245 
246  CollisionObject o1(s1, tf1);
247  CollisionObject o2(s2, tf2);
248 
249  // Enable computation of nearest points
250  DistanceRequest distanceRequest(true);
251  DistanceResult distanceResult;
252 
253  distance(&o1, &o2, distanceRequest, distanceResult);
254 
255  std::cerr << "Applied translation on two capsules";
256  std::cerr << " T1 = " << tf1.getTranslation()
257  << ", T2 = " << tf2.getTranslation() << std::endl;
258  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
259  << ", p2 = " << distanceResult.nearest_points[1]
260  << ", distance = " << distanceResult.min_distance << std::endl;
261 
262  BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
263 }
264 
265 BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) {
266  CollisionGeometryPtr_t s1(new Capsule(5, 10));
267  CollisionGeometryPtr_t s2(new Capsule(5, 10));
268 
269  Transform3f tf1;
270  Transform3f tf2(Vec3f(20, 20, 0));
271 
272  CollisionObject o1(s1, tf1);
273  CollisionObject o2(s2, tf2);
274 
275  // Enable computation of nearest points
276  DistanceRequest distanceRequest(true);
277  DistanceResult distanceResult;
278 
279  distance(&o1, &o2, distanceRequest, distanceResult);
280 
281  std::cerr << "Applied translation on two capsules";
282  std::cerr << " T1 = " << tf1.getTranslation()
283  << ", T2 = " << tf2.getTranslation() << std::endl;
284  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
285  << ", p2 = " << distanceResult.nearest_points[1]
286  << ", distance = " << distanceResult.min_distance << std::endl;
287 
288  FCL_REAL expected = sqrt(800) - 10;
289  BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6);
290 }
291 
292 BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) {
293  CollisionGeometryPtr_t s1(new Capsule(5, 10));
294  CollisionGeometryPtr_t s2(new Capsule(5, 10));
295 
296  Transform3f tf1;
297  Transform3f tf2(Vec3f(0, 0, 20.1));
298 
299  CollisionObject o1(s1, tf1);
300  CollisionObject o2(s2, tf2);
301 
302  // Enable computation of nearest points
303  DistanceRequest distanceRequest(true);
304  DistanceResult distanceResult;
305 
306  distance(&o1, &o2, distanceRequest, distanceResult);
307 
308  std::cerr << "Applied translation on two capsules";
309  std::cerr << " T1 = " << tf1.getTranslation()
310  << ", T2 = " << tf2.getTranslation() << std::endl;
311  std::cerr << "Closest points: p1 = " << distanceResult.nearest_points[0]
312  << ", p2 = " << distanceResult.nearest_points[1]
313  << ", distance = " << distanceResult.min_distance << std::endl;
314 
315  BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6);
316 }
317 
318 BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) {
319  CollisionGeometryPtr_t s1(new Capsule(5, 10));
320  CollisionGeometryPtr_t s2(new Capsule(5, 10));
321 
322  Transform3f tf1;
323  Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1));
324 
325  CollisionObject o1(s1, tf1);
326  CollisionObject o2(s2, tf2);
327 
328  // Enable computation of nearest points
329  DistanceRequest distanceRequest(true);
330  DistanceResult distanceResult;
331 
332  distance(&o1, &o2, distanceRequest, distanceResult);
333 
334  std::cerr << "Applied rotation and translation on two capsules" << std::endl;
335  std::cerr << "R1 = " << tf1.getRotation() << std::endl
336  << "T1 = " << tf1.getTranslation().transpose() << std::endl
337  << "R2 = " << tf2.getRotation() << std::endl
338  << "T2 = " << tf2.getTranslation().transpose() << std::endl;
339  std::cerr << "Closest points:" << std::endl
340  << "p1 = " << distanceResult.nearest_points[0].transpose()
341  << std::endl
342  << "p2 = " << distanceResult.nearest_points[1].transpose()
343  << std::endl
344  << "distance = " << distanceResult.min_distance << std::endl;
345 
346  const Vec3f& p1 = distanceResult.nearest_points[0];
347  const Vec3f& p2 = distanceResult.nearest_points[1];
348 
349  BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
350  CHECK_CLOSE_TO_0(p1[0], 1e-4);
351  CHECK_CLOSE_TO_0(p1[1], 1e-4);
352  BOOST_CHECK_CLOSE(p1[2], 10, 1e-4);
353  CHECK_CLOSE_TO_0(p2[0], 1e-4);
354  CHECK_CLOSE_TO_0(p2[1], 1e-4);
355  BOOST_CHECK_CLOSE(p2[2], 20.1, 1e-4);
356 }
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
collision_object.h
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
utility.h
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::makeQuat
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: utility.cpp:368
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
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::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0.
Definition: collision_data.h:424
hpp::fcl
Definition: broadphase_bruteforce.h:45
octree.p1
tuple p1
Definition: octree.py:54
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
transform.h
geometric_shapes.h
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial)
Definition: test/capsule_capsule.cpp:56
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
c2
c2
CHECK_CLOSE_TO_0
#define CHECK_CLOSE_TO_0(x, eps)
Definition: test/capsule_capsule.cpp:41
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312
collision.h

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:13