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::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
octree.p1
tuple p1
Definition: octree.py:55
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
geometric_shapes.h
hpp::fcl::makeQuat
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: utility.cpp:368
CHECK_CLOSE_TO_0
#define CHECK_CLOSE_TO_0(x, eps)
Definition: test/capsule_capsule.cpp:41
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
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::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
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::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
hpp::fcl::Transform3f::setRotation
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:136
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
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
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial)
Definition: test/capsule_capsule.cpp:56
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, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
Definition: include/hpp/fcl/fwd.hh:96
c2
c2
collision_object.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00