Debian Codegraph @

unittests/integration/test_Collision.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
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 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/collision/collision.hpp"
38 #include "dart/collision/fcl/fcl.hpp"
39 #include "dart/common/common.hpp"
40 #include "dart/config.hpp"
41 #include "dart/dynamics/dynamics.hpp"
42 #include "dart/math/math.hpp"
43 #if HAVE_ODE
44 #include "dart/collision/ode/ode.hpp"
45 #endif
46 #if HAVE_BULLET
47 #include "dart/collision/bullet/bullet.hpp"
48 #endif
49 #include "dart/simulation/simulation.hpp"
50 #include "dart/utils/utils.hpp"
51
52 #include "TestHelpers.hpp"
53
54 using namespace dart;
55 using namespace common;
56 using namespace math;
57 using namespace collision;
58 using namespace dynamics;
59 using namespace simulation;
60 using namespace utils;
61
62 class Collision : public testing::Test
63 {
64 public:
65 void unrotatedTest(
66 dart::collision::fcl::CollisionGeometry* _coll1,
67 dart::collision::fcl::CollisionGeometry* _coll2,
68 double expectedContactPoint,
69 int _idxAxis);
70 void dropWithRotation(
71 dart::collision::fcl::CollisionGeometry* _object,
72 double EulerZ,
73 double EulerY,
74 double EulerX);
75 void printResult(const dart::collision::fcl::CollisionResult& _result);
76 };
77
78 void Collision::unrotatedTest(
79 dart::collision::fcl::CollisionGeometry* _coll1,
80 dart::collision::fcl::CollisionGeometry* _coll2,
81 double expectedContactPoint,
82 int _idxAxis)
83 {
84 dart::collision::fcl::CollisionResult result;
85 dart::collision::fcl::CollisionRequest request;
86 request.enable_contact = true;
87 request.num_max_contacts = 100;
88
89 dart::collision::fcl::Vector3 position(0, 0, 0);
90
91 dart::collision::fcl::Transform3 coll1_transform;
92 dart::collision::fcl::Transform3 coll2_transform;
93
94 //==========================================================================
95 // Approaching test
96 //==========================================================================
97 result.clear();
98 double dpos = -0.001;
99 double pos = 10.0;
100
101 coll1_transform.setIdentity();
102 dart::collision::fcl::setTranslation(
103 coll1_transform, dart::collision::fcl::Vector3(0, 0, 0));
104 coll2_transform.setIdentity();
105
106 // Let's drop box2 until it collide with box1
107 do
108 {
109 position[_idxAxis] = pos;
110 dart::collision::fcl::setTranslation(coll2_transform, position);
111
112 ::fcl::collide(
113 _coll1, coll1_transform, _coll2, coll2_transform, request, result);
114
115 pos += dpos;
116 } while (result.numContacts() == 0);
117
118 //
119 if (_idxAxis == 0)
120 std::cout << "The object is collided when its x-axis position is: "
121 << (pos - dpos) << std::endl;
122 if (_idxAxis == 1)
123 std::cout << "The object is collided when its y-axis position is: "
124 << (pos - dpos) << std::endl;
125 if (_idxAxis == 2)
126 std::cout << "The object is collided when its z-axis position is: "
127 << (pos - dpos) << std::endl;
128
129 // printResult(result);
130
131 for (std::size_t i = 0; i < result.numContacts(); ++i)
132 {
133 EXPECT_GE(result.getContact(i).penetration_depth, 0.0);
134 // EXPECT_NEAR(result.getContact(i).normal[_idxAxis], -1.0);
135 EXPECT_EQ(dart::collision::fcl::length(result.getContact(i).normal), 1.0);
136 EXPECT_NEAR(
137 result.getContact(i).pos[_idxAxis], expectedContactPoint, -dpos * 2.0);
138 }
139 }
140
141 void Collision::dropWithRotation(
142 dart::collision::fcl::CollisionGeometry* _object,
143 double EulerZ,
144 double EulerY,
145 double EulerX)
146 {
147 // Collision test setting
148 dart::collision::fcl::CollisionResult result;
149 dart::collision::fcl::CollisionRequest request;
150 request.enable_contact = true;
151 request.num_max_contacts = 100;
152
153 // Ground like box setting
154 dart::collision::fcl::Box groundObject(100, 100, 0.1);
155 dart::collision::fcl::Transform3 groundTransf;
156 groundTransf.setIdentity();
157 dart::collision::fcl::Vector3 ground_position(0, 0, 0);
158 dart::collision::fcl::setTranslation(groundTransf, ground_position);
159
160 // Dropping object setting
161 dart::collision::fcl::Transform3 objectTransf;
162 dart::collision::fcl::Matrix3 rot;
163 dart::collision::fcl::setEulerZYX(rot, EulerZ, EulerY, EulerX);
164 dart::collision::fcl::setRotation(objectTransf, rot);
165 dart::collision::fcl::Vector3 dropping_position(0, 0, 0);
166 dart::collision::fcl::setTranslation(objectTransf, dropping_position);
167
168 //==========================================================================
169 // Dropping test in x, y, z aixs each.
170 //==========================================================================
171 for (int _idxAxis = 0; _idxAxis < 3; ++_idxAxis)
172 {
173 result.clear();
174
175 groundObject.side = dart::collision::fcl::Vector3(100, 100, 100);
176 groundObject.side[_idxAxis] = 0.1;
177 ground_position = dart::collision::fcl::Vector3(0, 0, 0);
178 ground_position[_idxAxis] = -0.05;
179 dart::collision::fcl::setTranslation(groundTransf, ground_position);
180
181 // Let's drop the object until it collide with ground
182 double posDelta = -0.0001;
183 double initPos = 10.0;
184 dropping_position = dart::collision::fcl::Vector3(0, 0, 0);
185 do
186 {
187 dropping_position[_idxAxis] = initPos;
188 dart::collision::fcl::setTranslation(objectTransf, dropping_position);
189
190 ::fcl::collide(
191 _object, objectTransf, &groundObject, groundTransf, request, result);
192
193 initPos += posDelta;
194 } while (result.numContacts() == 0);
195
196 std::cout << "Current position of the object: "
197 << dart::collision::fcl::getTranslation(objectTransf) << std::endl
198 << "Number of contacts: " << result.numContacts() << std::endl;
199
200 dart::collision::fcl::Transform3 objectTransfInv = objectTransf;
201 objectTransfInv.inverse();
202 for (std::size_t i = 0; i < result.numContacts(); ++i)
203 {
204 dart::collision::fcl::Vector3 posWorld = dart::collision::fcl::transform(
205 objectTransfInv, result.getContact(i).pos);
206 std::cout << "----- CONTACT " << i << " --------" << std::endl;
207 std::cout << "contact_points: " << result.getContact(i).pos << std::endl;
208 std::cout << "contact_points(w): " << posWorld << std::endl;
209 std::cout << "norm: "
210 << dart::collision::fcl::length(result.getContact(i).pos)
211 << std::endl;
212 std::cout << "penetration_depth: "
213 << result.getContact(i).penetration_depth << std::endl;
214 std::cout << "normal: " << result.getContact(i).normal << std::endl;
215 }
216
217 std::cout << std::endl;
218 }
219 }
220
221 void Collision::printResult(
222 const dart::collision::fcl::CollisionResult& _result)
223 {
224 std::cout << "====== [ RESULT ] ======" << std::endl;
225 std::cout << "The number of contacts: " << _result.numContacts() << std::endl;
226
227 for (std::size_t i = 0; i < _result.numContacts(); ++i)
228 {
229 std::cout << "----- CONTACT " << i << " --------" << std::endl;
230 std::cout << "contact_points: " << _result.getContact(i).pos << std::endl;
231 std::cout << "penetration_depth: "
232 << _result.getContact(i).penetration_depth << std::endl;
233 std::cout << "normal: " << _result.getContact(i).normal << std::endl;
234 // std::cout << std::endl;
235 }
236 std::cout << std::endl;
237 }
238
239 TEST_F(Collision, DROP)
240 {
241 dtdbg << "Unrotated box\n";
242 dart::collision::fcl::Box box1(0.5, 0.5, 0.5);
243 dropWithRotation(&box1, 0, 0, 0);
244
245 dtdbg << "Rotated box\n";
246 dart::collision::fcl::Box box2(0.5, 0.5, 0.5);
247 dropWithRotation(
248 &box2,
249 dart::math::Random::uniform(-3.14, 3.14),
250 dart::math::Random::uniform(-3.14, 3.14),
251 dart::math::Random::uniform(-3.14, 3.14));
252
253 dropWithRotation(&box2, 0.0, 0.1, 0.0);
254 }
255
256 TEST_F(Collision, FCL_BOX_BOX)
257 {
258 double EulerZ = 1;
259 double EulerY = 2;
260 double EulerX = 3;
261
262 // Collision test setting
263 dart::collision::fcl::CollisionResult result;
264 dart::collision::fcl::CollisionRequest request;
265 request.enable_contact = true;
266 request.num_max_contacts = 100;
267
268 // Ground like box setting
269 dart::collision::fcl::Box groundObject(100, 100, 0.1);
270 dart::collision::fcl::Transform3 groundTransf;
271 groundTransf.setIdentity();
272 dart::collision::fcl::Vector3 ground_position(0.0, 0.0, -0.05);
273 dart::collision::fcl::setTranslation(groundTransf, ground_position);
274
275 // Dropping box object setting
276 dart::collision::fcl::Box box(0.5, 0.5, 0.5);
277 dart::collision::fcl::Transform3 objectTransf;
278 dart::collision::fcl::Matrix3 rot;
279 dart::collision::fcl::setEulerZYX(rot, EulerZ, EulerY, EulerX);
280 dart::collision::fcl::setRotation(objectTransf, rot);
281 dart::collision::fcl::Vector3 dropping_position(0.0, 0.0, 5.0);
282 dart::collision::fcl::setTranslation(objectTransf, dropping_position);
283
284 // Let's drop the object until it collide with ground
285 do
286 {
287 dart::collision::fcl::setTranslation(objectTransf, dropping_position);
288
289 ::fcl::collide(
290 &box, objectTransf, &groundObject, groundTransf, request, result);
291
292 dropping_position[2] -= 0.00001;
293 } while (result.numContacts() == 0);
294
295 std::cout << "Current position of the object: "
296 << dart::collision::fcl::getTranslation(objectTransf) << std::endl
297 << "Number of contacts: " << result.numContacts() << std::endl;
298
299 for (std::size_t i = 0; i < result.numContacts(); ++i)
300 {
301 std::cout << "----- CONTACT " << i << " --------" << std::endl;
302 std::cout << "contact_points: " << result.getContact(i).pos << std::endl;
303 std::cout << "penetration_depth: " << result.getContact(i).penetration_depth
304 << std::endl;
305 std::cout << "normal: " << result.getContact(i).normal << std::endl;
306 }
307 }
308
309 //==============================================================================
310 void testSimpleFrames(const std::shared_ptr<CollisionDetector>& cd)
311 {
312 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
313 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
314 auto simpleFrame3 = SimpleFrame::createShared(Frame::World());
315
316 ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
317 ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
318 ShapePtr shape3(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
319
320 simpleFrame1->setShape(shape1);
321 simpleFrame2->setShape(shape2);
322 simpleFrame3->setShape(shape3);
323
324 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
325 auto group2 = cd->createCollisionGroup(simpleFrame2.get());
326 auto group3 = cd->createCollisionGroup(simpleFrame3.get());
327
328 auto groupAll
329 = cd->createCollisionGroup(group1.get(), group2.get(), group3.get());
330
331 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
332 EXPECT_EQ(group2->getNumShapeFrames(), 1u);
333 EXPECT_EQ(group3->getNumShapeFrames(), 1u);
334 EXPECT_EQ(
335 groupAll->getNumShapeFrames(),
336 group1->getNumShapeFrames() + group2->getNumShapeFrames()
337 + group3->getNumShapeFrames());
338
339 for (std::size_t i = 0; i < group1->getNumShapeFrames(); ++i)
340 EXPECT_EQ(groupAll->getShapeFrame(i), group1->getShapeFrame(i));
341
342 std::size_t start = group1->getNumShapeFrames();
343 std::size_t end = start + group2->getNumShapeFrames();
344 for (std::size_t i = start; i < end; ++i)
345 EXPECT_EQ(groupAll->getShapeFrame(i), group2->getShapeFrame(i - start));
346
347 start = start + group2->getNumShapeFrames();
348 end = start + group3->getNumShapeFrames();
349 for (std::size_t i = start; i < end; ++i)
350 EXPECT_EQ(groupAll->getShapeFrame(i), group3->getShapeFrame(i - start));
351
352 collision::CollisionOption option;
353 collision::CollisionResult result;
354
355 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
356 simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0));
357 simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0));
358 EXPECT_FALSE(group1->collide(option, &result));
359 EXPECT_FALSE(group2->collide(option, &result));
360 EXPECT_FALSE(group3->collide(option, &result));
361 EXPECT_FALSE(groupAll->collide(option, &result));
362
363 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
364 simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0));
365 simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0));
366 EXPECT_TRUE(group1->collide(group2.get(), option, &result));
367 EXPECT_TRUE(group1->collide(group2.get(), option, &result));
368 EXPECT_TRUE(group2->collide(group3.get(), option, &result));
369 EXPECT_TRUE(groupAll->collide(option, &result));
370
371 auto group23
372 = cd->createCollisionGroup(simpleFrame2.get(), simpleFrame3.get());
373 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
374 simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0));
375 simpleFrame3->setTranslation(Eigen::Vector3d(1.6, 0.0, 0.0));
376 EXPECT_FALSE(group1->collide(group2.get()));
377 EXPECT_FALSE(group1->collide(group3.get()));
378 EXPECT_TRUE(group2->collide(group3.get()));
379 EXPECT_TRUE(group23->collide());
380 EXPECT_FALSE(group1->collide(group23.get()));
381 }
382
383 //==============================================================================
384 TEST_F(Collision, SimpleFrames)
385 {
386 auto fcl_mesh_dart = FCLCollisionDetector::create();
387 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
388 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
389 testSimpleFrames(fcl_mesh_dart);
390
391 // auto fcl_prim_fcl = FCLCollisionDetector::create();
392 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
393 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
394 // testSimpleFrames(fcl_prim_fcl);
395
396 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
397 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
398 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
399 // testSimpleFrames(fcl_mesh_fcl);
400
401 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
402 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
403 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
404 // testSimpleFrames(fcl_mesh_fcl);
405
406 #if HAVE_BULLET
407 auto bullet = BulletCollisionDetector::create();
408 testSimpleFrames(bullet);
409 #endif
410
411 auto dart = DARTCollisionDetector::create();
412 testSimpleFrames(dart);
413 }
414
415 //==============================================================================
416 void testSphereSphere(
417 const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
418 {
419 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
420 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
421
422 ShapePtr shape1(new SphereShape(1.0));
423 ShapePtr shape2(new SphereShape(0.5));
424 simpleFrame1->setShape(shape1);
425 simpleFrame2->setShape(shape2);
426
427 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
428
429 EXPECT_EQ(group->getNumShapeFrames(), 2u);
430
431 collision::CollisionOption option;
432 option.enableContact = true;
433
434 collision::CollisionResult result;
435
436 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
437 simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0));
438
439 //----------------------------------------------------------------------------
440 // Test 1: No contact
441 //----------------------------------------------------------------------------
442
443 result.clear();
444 EXPECT_FALSE(group->collide(option, &result));
445 EXPECT_TRUE(result.getNumContacts() == 0u);
446
447 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
448 simpleFrame2->setTranslation(Eigen::Vector3d(1.5, 0.0, 0.0));
449
450 //----------------------------------------------------------------------------
451 // Test 2: Point contact
452 //----------------------------------------------------------------------------
453
454 result.clear();
455 EXPECT_TRUE(group->collide(option, &result));
456 EXPECT_TRUE(result.getNumContacts() == 1u);
457
458 const auto& contact = result.getContact(0);
459
460 // Test contact location
461 EXPECT_TRUE(contact.point.isApprox(Eigen::Vector3d::UnitX(), tol));
462
463 // Test normal
464 Eigen::Vector3d expectedNormal;
465 if (result.getContact(0).collisionObject1->getShapeFrame()
466 == simpleFrame1.get())
467 expectedNormal << -1, 0, 0;
468 else
469 expectedNormal << 1, 0, 0;
470 double tol2 = tol;
471 if (cd->getType() == FCLCollisionDetector::getStaticType()
472 && static_cast<FCLCollisionDetector*>(cd.get())->getPrimitiveShapeType()
473 == FCLCollisionDetector::MESH)
474 {
475 tol2 *= 1e+12;
476 // FCL returns less accurate contact normals for sphere-sphere since we're
477 // using sphere-like rough meshes instead of analytical sphere shapes.
478 }
479 EXPECT_TRUE(contact.normal.isApprox(expectedNormal, tol2));
480
481 //----------------------------------------------------------------------------
482 // Test 3: Corner case of that the bigger sphere completely encloses the
483 // smaller sphere
484 //----------------------------------------------------------------------------
485
486 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
487 simpleFrame2->setTranslation(Eigen::Vector3d::Zero());
488 result.clear();
489 if (cd->getType() == FCLCollisionDetector::getStaticType())
490 {
491 EXPECT_FALSE(group->collide(option, &result));
492 // FCL is not able to detect collisions when an object completely (strictly)
493 // contanins the other object (no collisions between the hulls)
494 }
495 else
496 {
497 EXPECT_TRUE(group->collide(option, &result));
498 // TODO(JS): BulletCollsionDetector includes a bug related to this.
499 // (see #876)
500 #if HAVE_BULLET
501 if (cd->getType() != BulletCollisionDetector::getStaticType())
502 #endif
503 {
504 EXPECT_EQ(result.getNumContacts(), 1u);
505 }
506 for (auto i = 0u; i < result.getNumContacts(); ++i)
507 {
508 std::cout << "point: " << result.getContact(i).point.transpose()
509 << std::endl;
510 }
511 }
512 // The positions of contact point are different depending on the collision
513 // detector. More integration tests need to be added.
514 }
515
516 //==============================================================================
517 TEST_F(Collision, SphereSphere)
518 {
519 auto fcl_mesh_dart = FCLCollisionDetector::create();
520 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
521 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
522 testSphereSphere(fcl_mesh_dart);
523
524 // auto fcl_prim_fcl = FCLCollisionDetector::create();
525 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
526 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
527 // testSphereSphere(fcl_prim_fcl);
528
529 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
530 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
531 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
532 // testSphereSphere(fcl_mesh_fcl);
533
534 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
535 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
536 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
537 // testSphereSphere(fcl_mesh_fcl);
538
539 #if HAVE_ODE
540 auto ode = OdeCollisionDetector::create();
541 testSphereSphere(ode);
542 #endif
543
544 #if HAVE_BULLET
545 auto bullet = BulletCollisionDetector::create();
546 testSphereSphere(bullet);
547 #endif
548
549 auto dart = DARTCollisionDetector::create();
550 testSphereSphere(dart);
551 }
552
553 //==============================================================================
554 bool checkBoundingBox(
555 const Eigen::Vector3d& min,
556 const Eigen::Vector3d& max,
557 const Eigen::Vector3d& point,
558 double tol = 1e-12)
559 {
560 for (auto i = 0u; i < 3u; ++i)
561 {
562 if (min[i] - tol > point[i] || point[i] > max[i] + tol)
563 return false;
564 }
565
566 return true;
567 }
568
569 //==============================================================================
570 void testBoxBox(
571 const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
572 {
573 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
574 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
575
576 ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
577 ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
578 simpleFrame1->setShape(shape1);
579 simpleFrame2->setShape(shape2);
580
581 Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, -0.5);
582 Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.5, 0.25);
583 simpleFrame1->setTranslation(pos1);
584 simpleFrame2->setTranslation(pos2);
585
586 auto group1 = cd->createCollisionGroup(simpleFrame1.get());
587 auto group2 = cd->createCollisionGroup(simpleFrame2.get());
588 auto groupAll = cd->createCollisionGroup(group1.get(), group2.get());
589
590 EXPECT_EQ(group1->getNumShapeFrames(), 1u);
591 EXPECT_EQ(group2->getNumShapeFrames(), 1u);
592 EXPECT_EQ(
593 groupAll->getNumShapeFrames(),
594 group1->getNumShapeFrames() + group2->getNumShapeFrames());
595
596 collision::CollisionOption option;
597 collision::CollisionResult result;
598
599 result.clear();
600 EXPECT_TRUE(group1->collide(group2.get(), option, &result));
601
602 result.clear();
603 EXPECT_TRUE(groupAll->collide(option, &result));
604
605 Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0);
606 Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0);
607
608 const auto numContacts = result.getNumContacts();
609
610 const auto checkNumContacts = (numContacts <= 4u);
611 EXPECT_TRUE(checkNumContacts);
612 if (!checkNumContacts)
613 std::cout << "# of contants: " << numContacts << "\n";
614
615 for (const auto& contact : result.getContacts())
616 {
617 const auto& point = contact.point;
618
619 const auto result = checkBoundingBox(min, max, point, tol);
620 EXPECT_TRUE(result);
621
622 if (!result)
623 std::cout << "point: " << point.transpose() << "\n";
624 }
625 }
626
627 //==============================================================================
628 TEST_F(Collision, BoxBox)
629 {
630 auto fcl_mesh_dart = FCLCollisionDetector::create();
631 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
632 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
633 testBoxBox(fcl_mesh_dart);
634
635 // auto fcl_prim_fcl = FCLCollisionDetector::create();
636 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
637 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
638 // testBoxBox(fcl_prim_fcl);
639
640 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
641 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
642 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
643 // testBoxBox(fcl_mesh_fcl);
644
645 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
646 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
647 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
648 // testBoxBox(fcl_mesh_fcl);
649
650 #if HAVE_ODE
651 auto ode = OdeCollisionDetector::create();
652 testBoxBox(ode);
653 #endif
654
655 #if HAVE_BULLET
656 auto bullet = BulletCollisionDetector::create();
657 testBoxBox(bullet);
658 #endif
659
660 auto dart = DARTCollisionDetector::create();
661 testBoxBox(dart);
662 }
663
664 //==============================================================================
665 void testOptions(const std::shared_ptr<CollisionDetector>& cd)
666 {
667 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
668 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
669 auto simpleFrame3 = SimpleFrame::createShared(Frame::World());
670
671 ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
672 ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
673 ShapePtr shape3(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
674 simpleFrame1->setShape(shape1);
675 simpleFrame2->setShape(shape2);
676 simpleFrame3->setShape(shape3);
677
678 Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, -0.5);
679 Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.5, 0.25);
680 Eigen::Vector3d pos3 = Eigen::Vector3d(0.0, -0.5, 0.25);
681 simpleFrame1->setTranslation(pos1);
682 simpleFrame2->setTranslation(pos2);
683 simpleFrame3->setTranslation(pos3);
684
685 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
686 EXPECT_EQ(group->getNumShapeFrames(), 2u);
687
688 collision::CollisionOption option;
689 collision::CollisionResult result;
690
691 result.clear();
692 option.maxNumContacts = 1000u;
693 EXPECT_TRUE(group->collide(option, &result));
694 EXPECT_EQ(result.getNumContacts(), 4u);
695
696 result.clear();
697 option.maxNumContacts = 2u;
698 EXPECT_TRUE(group->collide(option, &result));
699 EXPECT_EQ(result.getNumContacts(), 2u);
700
701 group->addShapeFrame(simpleFrame3.get());
702 result.clear();
703 option.maxNumContacts = 1u;
704 EXPECT_TRUE(group->collide(option, &result));
705 EXPECT_EQ(result.getNumContacts(), 1u);
706
707 // Binary check without passing result
708 EXPECT_TRUE(group->collide(option));
709
710 // Binary check without passing option and result
711 EXPECT_TRUE(group->collide());
712
713 // Zero maximum number of contacts
714 option.maxNumContacts = 0u;
715 option.enableContact = true;
716 EXPECT_TRUE(group->collide());
717 EXPECT_FALSE(group->collide(option));
718 EXPECT_FALSE(group->collide(option, nullptr));
719 EXPECT_FALSE(group->collide(option, &result));
720 EXPECT_EQ(result.getNumContacts(), 0u);
721 EXPECT_FALSE(result.isCollision());
722 }
723
724 //==============================================================================
725 void testCylinderCylinder(const std::shared_ptr<CollisionDetector>& cd)
726 {
727 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
728 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
729
730 auto shape1 = std::make_shared<CylinderShape>(1.0, 1.0);
731 auto shape2 = std::make_shared<CylinderShape>(0.5, 1.0);
732
733 simpleFrame1->setShape(shape1);
734 simpleFrame2->setShape(shape2);
735
736 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
737
738 EXPECT_EQ(group->getNumShapeFrames(), 2u);
739
740 collision::CollisionOption option;
741 option.enableContact = true;
742
743 collision::CollisionResult result;
744
745 result.clear();
746 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
747 simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0));
748 EXPECT_FALSE(group->collide(option, &result));
749 EXPECT_TRUE(result.getNumContacts() == 0u);
750
751 result.clear();
752 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
753 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
754 EXPECT_TRUE(group->collide(option, &result));
755 EXPECT_TRUE(result.getNumContacts() >= 1u);
756 }
757
758 //==============================================================================
759 TEST_F(Collision, testCylinderCylinder)
760 {
761 auto fcl_mesh_dart = FCLCollisionDetector::create();
762 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
763 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
764 testCylinderCylinder(fcl_mesh_dart);
765
766 // auto fcl_prim_fcl = FCLCollisionDetector::create();
767 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
768 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
769 // testCylinderCylinder(fcl_prim_fcl);
770
771 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
772 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
773 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
774 // testCylinderCylinder(fcl_mesh_fcl);
775
776 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
777 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
778 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
779 // testCylinderCylinder(fcl_mesh_fcl);
780
781 #if HAVE_ODE
782 auto ode = OdeCollisionDetector::create();
783 testCylinderCylinder(ode);
784 #endif
785
786 #if HAVE_BULLET
787 auto bullet = BulletCollisionDetector::create();
788 testCylinderCylinder(bullet);
789 #endif
790
791 // auto dart = DARTCollisionDetector::create();
792 // testCylinderCylinder(dart);
793 }
794
795 //==============================================================================
796 void testConeCone(const std::shared_ptr<CollisionDetector>& cd)
797 {
798 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
799 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
800
801 auto shape1 = std::make_shared<ConeShape>(1.0, 1.0);
802 auto shape2 = std::make_shared<ConeShape>(0.5, 1.0);
803
804 simpleFrame1->setShape(shape1);
805 simpleFrame2->setShape(shape2);
806
807 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
808
809 EXPECT_EQ(group->getNumShapeFrames(), 2u);
810
811 collision::CollisionOption option;
812 option.enableContact = true;
813
814 collision::CollisionResult result;
815
816 result.clear();
817 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
818 simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0));
819 EXPECT_FALSE(group->collide(option, &result));
820 EXPECT_TRUE(result.getNumContacts() == 0u);
821
822 result.clear();
823 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
824 simpleFrame2->setTranslation(Eigen::Vector3d(0.75, 0.0, 0.0));
825 EXPECT_TRUE(group->collide(option, &result));
826 EXPECT_TRUE(result.getNumContacts() >= 1u);
827 }
828
829 //==============================================================================
830 TEST_F(Collision, testConeCone)
831 {
832 auto fcl_mesh_dart = FCLCollisionDetector::create();
833 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
834 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
835 testCylinderCylinder(fcl_mesh_dart);
836
837 auto fcl_mesh_fcl = FCLCollisionDetector::create();
838 fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
839 fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
840 testCylinderCylinder(fcl_mesh_fcl);
841
842 auto fcl_prim_dart = FCLCollisionDetector::create();
843 fcl_prim_dart->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
844 fcl_prim_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
845 testCylinderCylinder(fcl_prim_dart);
846
847 auto fcl_prim_fcl = FCLCollisionDetector::create();
848 fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
849 fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
850 testCylinderCylinder(fcl_prim_fcl);
851
852 #if HAVE_ODE
853 auto ode = OdeCollisionDetector::create();
854 testCylinderCylinder(ode);
855 #endif
856
857 #if HAVE_BULLET
858 auto bullet = BulletCollisionDetector::create();
859 testCylinderCylinder(bullet);
860 #endif
861
862 // auto dart = DARTCollisionDetector::create();
863 // testCylinderCylinder(dart);
864 }
865
866 //==============================================================================
867 void testCapsuleCapsule(const std::shared_ptr<CollisionDetector>& cd)
868 {
869 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
870 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
871
872 auto shape1 = std::make_shared<CapsuleShape>(1.0, 1.0);
873 auto shape2 = std::make_shared<CapsuleShape>(0.5, 1.0);
874
875 simpleFrame1->setShape(shape1);
876 simpleFrame2->setShape(shape2);
877
878 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
879
880 EXPECT_EQ(group->getNumShapeFrames(), 2u);
881
882 collision::CollisionOption option;
883 option.enableContact = true;
884
885 collision::CollisionResult result;
886
887 result.clear();
888 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
889 simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0));
890 EXPECT_FALSE(group->collide(option, &result));
891 EXPECT_TRUE(result.getNumContacts() == 0u);
892
893 result.clear();
894 simpleFrame1->setTranslation(Eigen::Vector3d::Zero());
895 simpleFrame2->setTranslation(Eigen::Vector3d(0.74, 0.0, 0.0));
896 EXPECT_TRUE(group->collide(option, &result));
897 EXPECT_TRUE(result.getNumContacts() >= 1u);
898 }
899
900 //==============================================================================
901 TEST_F(Collision, testCapsuleCapsule)
902 {
903 // auto fcl_mesh_dart = FCLCollisionDetector::create();
904 // fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
905 // fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
906 // testCapsuleCapsule(fcl_mesh_dart);
907
908 // auto fcl_prim_fcl = FCLCollisionDetector::create();
909 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
910 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
911 // testCapsuleCapsule(fcl_prim_fcl);
912
913 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
914 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
915 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
916 // testCapsuleCapsule(fcl_mesh_fcl);
917
918 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
919 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
920 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
921 // testCapsuleCapsule(fcl_mesh_fcl);
922
923 #if HAVE_ODE
924 auto ode = OdeCollisionDetector::create();
925 testCapsuleCapsule(ode);
926 #endif
927
928 #if HAVE_BULLET
929 auto bullet = BulletCollisionDetector::create();
930 testCapsuleCapsule(bullet);
931 #endif
932
933 // auto dart = DARTCollisionDetector::create();
934 // testCapsuleCapsule(dart);
935 }
936
937 //==============================================================================
938 void testPlane(const std::shared_ptr<CollisionDetector>& cd)
939 {
940 auto planeFrame = SimpleFrame::createShared(Frame::World());
941 auto sphereFrame = SimpleFrame::createShared(Frame::World());
942 auto boxFrame = SimpleFrame::createShared(Frame::World());
943
944 auto plane = std::make_shared<PlaneShape>(Eigen::Vector3d::UnitZ(), 0.0);
945 auto sphere = std::make_shared<SphereShape>(0.5);
946 auto box = std::make_shared<BoxShape>(Eigen::Vector3d(1.0, 1.0, 1.0));
947
948 planeFrame->setShape(plane);
949 sphereFrame->setShape(sphere);
950 boxFrame->setShape(box);
951
952 auto group = cd->createCollisionGroup(
953 planeFrame.get(), sphereFrame.get(), boxFrame.get());
954
955 EXPECT_EQ(group->getNumShapeFrames(), 3u);
956
957 collision::CollisionOption option;
958 option.enableContact = true;
959
960 collision::CollisionResult result;
961
962 result.clear();
963 sphereFrame->setTranslation(Eigen::Vector3d(-10.0, 0.0, 1.0));
964 boxFrame->setTranslation(Eigen::Vector3d(-8.0, 0.0, 1.0));
965 EXPECT_FALSE(group->collide(option, &result));
966
967 result.clear();
968 sphereFrame->setTranslation(Eigen::Vector3d(-10.0, 0.0, 0.49));
969 boxFrame->setTranslation(Eigen::Vector3d(-8.0, 0.0, 0.49));
970 EXPECT_TRUE(group->collide(option, &result));
971 }
972
973 //==============================================================================
974 TEST_F(Collision, testPlane)
975 {
976 #if HAVE_ODE
977 auto ode = OdeCollisionDetector::create();
978 testPlane(ode);
979 #endif
980 }
981
982 //==============================================================================
983 /// \param[in] collidesUnderTerrain Set to true if the collision engine returns
984 /// collisions when a shape is underneath the terrain, but still above the
985 /// minimum height. If false, only intersections with the surface mesh will be
986 /// detected.
987 /// \param[in] extendsUntilGroundPlane Set to true if the collision engine
988 /// extends the terrain until the plane z=0
989 /// \param[in] odeThck: for ODE, use this thickness underneath the heightfield
990 /// to adjust collision checks.
991 ///
992 /// \sa dGeomHeightfieldDataBuild*().
993 template <typename S>
994 void testHeightmapBox(
995 CollisionDetector* cd,
996 const bool collidesUnderTerrain = true,
997 const bool extendsUntilGroundPlane = false,
998 const S odeThck = 0)
999 {
1000 using Vector3 = Eigen::Matrix<S, 3, 1>;
1001
1002 ///////////////////////////////////////
1003 // Set test parameters.
1004 // The height field will have a flat, even
1005 // slope spanned by four corner vertices
1006 ///////////////////////////////////////
1007
1008 // size of box
1009 const S boxSize = S(0.1);
1010 // terrain scale in x and y direction
1011 const S terrainScale = S(2.0);
1012 // z values scale
1013 const S zScale = S(2.0);
1014
1015 // minimum hand maximum height of terrain to use
1016 const S minH = 1.0; // note: ODE doesn't behave well with negative heights
1017 const S maxH = 3.0;
1018 // adjusted minimum height: If minH > 0, and extendsUntilGroundPlane true,
1019 // then the minimum height is actually 0.
1020 const S adjMinH = (extendsUntilGroundPlane && (minH > S(0))) ? 0.0 : minH;
1021 const S halfHeight = minH + (maxH - minH) / S(2);
1022 // ODE thickness is only used if there is not already a layer of this
1023 // thickness due to a minH > 0 (for ODE, extendsUntilGroundPlane is true)
1024 const S useOdeThck
1025 = (odeThck > S(1.0e-06)) ? std::max(odeThck - minH, S(0)) : 0.0;
1026
1027 ///////////////////////////////////////
1028 // Create frames and shapes
1029 ///////////////////////////////////////
1030
1031 // frames and shapes
1032 auto terrainFrame = SimpleFrame::createShared(Frame::World());
1033 auto boxFrame = SimpleFrame::createShared(Frame::World());
1034 auto terrainShape = std::make_shared<HeightmapShape<S>>();
1035 auto boxShape = std::make_shared<BoxShape>(
1036 Eigen::Vector3d::Constant(static_cast<double>(boxSize)));
1037
1038 // make a terrain with a linearly increasing slope
1039 std::vector<S> heights = {minH, halfHeight, halfHeight, maxH};
1040 terrainShape->setHeightField(2u, 2u, heights);
1041 // set a scale to test this at the same time
1042 const S terrSize = terrainScale;
1043 terrainShape->setScale(Vector3(terrainScale, terrainScale, zScale));
1044 EXPECT_EQ(terrainShape->getHeightField().size(), heights.size());
1045
1046 terrainFrame->setShape(terrainShape);
1047 boxFrame->setShape(boxShape);
1048
1049 ///////////////////////////////////////
1050 // Test collisions
1051 ///////////////////////////////////////
1052
1053 auto group = cd->createCollisionGroup(terrainFrame.get(), boxFrame.get());
1054 EXPECT_EQ(group->getNumShapeFrames(), 2u);
1055
1056 collision::CollisionOption option;
1057 option.enableContact = true;
1058
1059 collision::CollisionResult result;
1060 // the terrain is going to remain in the origin. During the tests,
1061 // we are only moving the box.
1062 terrainFrame->setTranslation(Eigen::Vector3d::Zero());
1063
1064 // there should be no collision underneath the height field, which should be
1065 // on the x/y plane.
1066 result.clear();
1067 // Some tolerance (useOdeThck) has to be added for ODE because it adds an
1068 // extra piece on the bottom to prevent objects from falling through
1069 // lowest points.
1070 S transZ = adjMinH * zScale - boxSize * S(0.501) - useOdeThck;
1071 boxFrame->setTranslation(Vector3(0.0, 0.0, transZ).template cast<double>());
1072 EXPECT_FALSE(group->collide(option, &result));
1073 EXPECT_EQ(result.getNumContacts(), 0u);
1074
1075 // expect collision if moved just slightly above the lower terrain bound
1076 if (collidesUnderTerrain)
1077 {
1078 result.clear();
1079 transZ = adjMinH * zScale - boxSize * S(0.499) - useOdeThck;
1080 boxFrame->setTranslation(Vector3(0.0, 0.0, transZ).template cast<double>());
1081 EXPECT_TRUE(group->collide(option, &result));
1082 EXPECT_GT(result.getNumContacts(), 0u);
1083 }
1084
1085 ///////////////////////////////////////
1086 // test collisions when box is at extreme corner
1087 // points (lowest and highest)
1088 ///////////////////////////////////////
1089
1090 // some helper vectors
1091 Vector3 slope(1.0, -1.0, maxH - minH);
1092 slope.normalize();
1093 Vector3 crossSection(1.0, 1.0, heights[1] - heights[2]);
1094 crossSection.normalize();
1095 const Vector3 normal = slope.cross(crossSection);
1096 // the two extreme corners:
1097 const Vector3 highCorner
1098 = Vector3(terrSize / S(2), -terrSize / S(2), maxH * zScale);
1099 const Vector3 lowCorner
1100 = Vector3(-terrSize / S(2), terrSize / S(2), maxH * zScale);
1101
1102 // ODE doesn't do nicely when boxes are close to the border of the terrain.
1103 // Shift the boxes along the slope (or normal to slope for some tests)
1104 // by this length.
1105 // Technically we should compute this a bit more accurately than this.
1106 // it basically has to ensure the box is inside or outside the terrain
1107 // bounds, so the slope plays a role for this factor.
1108 // But since the box is small, an estimate is used for now.
1109 const S boxShift = boxSize * S(1.5);
1110
1111 // expect collision at highest point (at max height)
1112 Vector3 cornerShift = highCorner - slope * boxShift;
1113 result.clear();
1114 boxFrame->setTranslation(cornerShift.template cast<double>());
1115 EXPECT_TRUE(group->collide(option, &result));
1116 EXPECT_GT(result.getNumContacts(), 0u);
1117
1118 // .. but not at opposite corner (lowest corner, at overall max height)
1119 result.clear();
1120 cornerShift = Vector3(lowCorner + slope * boxShift);
1121 boxFrame->setTranslation(cornerShift.template cast<double>());
1122 EXPECT_FALSE(group->collide(option, &result));
1123 EXPECT_EQ(result.getNumContacts(), 0u);
1124
1125 ///////////////////////////////////////
1126 // test collisions for box on z axis
1127 ///////////////////////////////////////
1128
1129 // box should collide where it intersects the slope
1130 result.clear();
1131 Vector3 inMiddle(0.0, 0.0, halfHeight * zScale);
1132 boxFrame->setTranslation(inMiddle.template cast<double>());
1133 // TODO(JS): Disabled temporarily
1134 if (cd->getType() != "bullet")
1135 {
1136 EXPECT_TRUE(group->collide(option, &result));
1137 EXPECT_GT(result.getNumContacts(), 0u);
1138 }
1139
1140 // ... but not if the box is translated away from the slope
1141 result.clear();
1142 Vector3 onTopOfSlope = inMiddle + normal * boxShift;
1143 boxFrame->setTranslation(onTopOfSlope.template cast<double>());
1144 EXPECT_FALSE(group->collide(option, &result));
1145 EXPECT_EQ(result.getNumContacts(), 0u);
1146
1147 // ... however it still should collide if translated the
1148 // other way inside the slope
1149 if (collidesUnderTerrain)
1150 {
1151 result.clear();
1152 Vector3 underSlope = inMiddle - normal * boxShift;
1153 boxFrame->setTranslation(underSlope.template cast<double>());
1154 EXPECT_TRUE(group->collide(option, &result));
1155 EXPECT_GT(result.getNumContacts(), 0u);
1156 }
1157 }
1158
1159 //==============================================================================
1160 TEST_F(Collision, testHeightmapBox)
1161 {
1162 #if HAVE_ODE
1163 auto ode = OdeCollisionDetector::create();
1164 // TODO take this message out as soon as testing is done
1165 dtdbg << "Testing ODE (float)" << std::endl;
1166 testHeightmapBox<float>(ode.get(), true, true, 0.05f);
1167
1168 // TODO take this message out as soon as testing is done
1169 dtdbg << "Testing ODE (double)" << std::endl;
1170 testHeightmapBox<double>(ode.get(), true, true, 0.05);
1171 #endif
1172
1173 #if HAVE_BULLET
1174 auto bullet = BulletCollisionDetector::create();
1175
1176 // TODO take this message out as soon as testing is done
1177 dtdbg << "Testing Bullet (float)" << std::endl;
1178 // bullet so far only supports float height fields, so don't test double here.
1179 testHeightmapBox<float>(bullet.get(), false, false);
1180 #endif
1181 }
1182
1183 //==============================================================================
1184 // Tests HeightmapShape::flipY();
1185 TEST_F(Collision, testHeightmapFlipY)
1186 {
1187 using S = double;
1188
1189 std::vector<S> heights1 = {-1, -2, 2, 1};
1190 auto shape = std::make_shared<HeightmapShape<S>>();
1191 shape->setHeightField(2, 2, heights1);
1192 shape->flipY();
1193 EXPECT_EQ(shape->getHeightField().data()[0], heights1[2]);
1194 EXPECT_EQ(shape->getHeightField().data()[1], heights1[3]);
1195 EXPECT_EQ(shape->getHeightField().data()[2], heights1[0]);
1196 EXPECT_EQ(shape->getHeightField().data()[3], heights1[1]);
1197
1198 // test with odd number of rows
1199 std::vector<S> heights2 = {-1, -2, 3, 3, 2, 1};
1200 shape->setHeightField(2, 3, heights2);
1201 shape->flipY();
1202 EXPECT_EQ(shape->getHeightField().data()[0], heights2[4]);
1203 EXPECT_EQ(shape->getHeightField().data()[1], heights2[5]);
1204 EXPECT_EQ(shape->getHeightField().data()[2], heights2[2]);
1205 EXPECT_EQ(shape->getHeightField().data()[3], heights2[3]);
1206 EXPECT_EQ(shape->getHeightField().data()[4], heights2[0]);
1207 EXPECT_EQ(shape->getHeightField().data()[5], heights2[1]);
1208
1209 // test higher number of rows
1210 std::vector<S> heights3 = {1, -1, 2, -2, 3, -3, 4, -4};
1211 shape->setHeightField(2, 4, heights3);
1212 shape->flipY();
1213 EXPECT_EQ(shape->getHeightField().data()[0], heights3[6]);
1214 EXPECT_EQ(shape->getHeightField().data()[1], heights3[7]);
1215 EXPECT_EQ(shape->getHeightField().data()[2], heights3[4]);
1216 EXPECT_EQ(shape->getHeightField().data()[3], heights3[5]);
1217 EXPECT_EQ(shape->getHeightField().data()[4], heights3[2]);
1218 EXPECT_EQ(shape->getHeightField().data()[5], heights3[3]);
1219 EXPECT_EQ(shape->getHeightField().data()[6], heights3[0]);
1220 EXPECT_EQ(shape->getHeightField().data()[7], heights3[1]);
1221
1222 // test wider rows
1223 std::vector<S> heights4 = {1, -1, 1.5, 2, -2, 2.5, 3, -3, 3.5, 4, -4, 4.5};
1224 shape->setHeightField(3, 4, heights4);
1225 shape->flipY();
1226 EXPECT_EQ(shape->getHeightField().data()[0], heights4[9]);
1227 EXPECT_EQ(shape->getHeightField().data()[1], heights4[10]);
1228 EXPECT_EQ(shape->getHeightField().data()[2], heights4[11]);
1229 EXPECT_EQ(shape->getHeightField().data()[3], heights4[6]);
1230 EXPECT_EQ(shape->getHeightField().data()[4], heights4[7]);
1231 EXPECT_EQ(shape->getHeightField().data()[5], heights4[8]);
1232 EXPECT_EQ(shape->getHeightField().data()[6], heights4[3]);
1233 EXPECT_EQ(shape->getHeightField().data()[7], heights4[4]);
1234 EXPECT_EQ(shape->getHeightField().data()[8], heights4[5]);
1235 EXPECT_EQ(shape->getHeightField().data()[9], heights4[0]);
1236 EXPECT_EQ(shape->getHeightField().data()[10], heights4[1]);
1237 EXPECT_EQ(shape->getHeightField().data()[11], heights4[2]);
1238
1239 // test mini (actually meaningless) height field
1240 std::vector<S> heights5 = {1, 2};
1241 shape->setHeightField(1, 2, heights5);
1242 shape->flipY();
1243 EXPECT_EQ(shape->getHeightField().data()[0], heights5[1]);
1244 EXPECT_EQ(shape->getHeightField().data()[1], heights5[0]);
1245
1246 // test height field with only one row (which is actually meaningless)
1247 std::vector<S> heights6 = {1, 2};
1248 shape->setHeightField(2, 1, heights6);
1249 shape->flipY();
1250 EXPECT_EQ(shape->getHeightField().data()[0], heights6[0]);
1251 EXPECT_EQ(shape->getHeightField().data()[1], heights6[1]);
1252
1253 // test height field with only one column (which is actually meaningless)
1254 std::vector<S> heights7 = {1, 2};
1255 shape->setHeightField(1, 2, heights7);
1256 shape->flipY();
1257 EXPECT_EQ(shape->getHeightField().data()[0], heights7[1]);
1258 EXPECT_EQ(shape->getHeightField().data()[1], heights7[0]);
1259
1260 // test height field with only one col and row (which is actually meaningless)
1261 std::vector<S> heights8 = {1};
1262 shape->setHeightField(1, 1, heights8);
1263 shape->flipY();
1264 EXPECT_EQ(shape->getHeightField().data()[0], heights8[0]);
1265 }
1266
1267 //==============================================================================
1268 TEST_F(Collision, Options)
1269 {
1270 auto fcl_mesh_dart = FCLCollisionDetector::create();
1271 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1272 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
1273 testOptions(fcl_mesh_dart);
1274
1275 // auto fcl_prim_fcl = FCLCollisionDetector::create();
1276 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1277 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1278 // testOptions(fcl_prim_fcl);
1279
1280 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1281 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1282 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
1283 // testOptions(fcl_mesh_fcl);
1284
1285 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1286 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1287 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1288 // testOptions(fcl_mesh_fcl);
1289
1290 #if HAVE_BULLET
1291 auto bullet = BulletCollisionDetector::create();
1292 testOptions(bullet);
1293 #endif
1294
1295 auto dart = DARTCollisionDetector::create();
1296 testOptions(dart);
1297 }
1298
1299 //==============================================================================
1300 void testFilter(const std::shared_ptr<CollisionDetector>& cd)
1301 {
1302 // Create two bodies skeleton. The two bodies are placed at the same position
1303 // with the same size shape so that they collide by default.
1304 auto skel = Skeleton::create();
1305 auto shape = std::make_shared<BoxShape>(Eigen::Vector3d(1, 1, 1));
1306 auto pair0 = skel->createJointAndBodyNodePair<RevoluteJoint>(nullptr);
1307 auto* body0 = pair0.second;
1308 body0->createShapeNodeWith<VisualAspect, CollisionAspect>(shape);
1309 auto pair1 = body0->createChildJointAndBodyNodePair<RevoluteJoint>();
1310 auto* body1 = pair1.second;
1311 body1->createShapeNodeWith<VisualAspect, CollisionAspect>(shape);
1312
1313 // Create a world and add the created skeleton
1314 auto world = std::make_shared<simulation::World>();
1315 auto constraintSolver = world->getConstraintSolver();
1316 constraintSolver->setCollisionDetector(cd);
1317 world->addSkeleton(skel);
1318
1319 // Get the collision group from the constraint solver
1320 auto group = constraintSolver->getCollisionGroup();
1321 EXPECT_EQ(group->getNumShapeFrames(), 2u);
1322
1323 // Default collision filter for Skeleton
1324 auto& option = constraintSolver->getCollisionOption();
1325 auto bodyNodeFilter = std::make_shared<BodyNodeCollisionFilter>();
1326 option.collisionFilter = bodyNodeFilter;
1327
1328 skel->enableSelfCollisionCheck();
1329 skel->enableAdjacentBodyCheck();
1330 EXPECT_TRUE(skel->isEnabledSelfCollisionCheck());
1331 EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck());
1332 EXPECT_TRUE(group->collide()); // without filter, always collision
1333 EXPECT_TRUE(group->collide(option));
1334
1335 skel->enableSelfCollisionCheck();
1336 skel->disableAdjacentBodyCheck();
1337 EXPECT_TRUE(skel->isEnabledSelfCollisionCheck());
1338 EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck());
1339 EXPECT_TRUE(group->collide());
1340 EXPECT_FALSE(group->collide(option));
1341
1342 skel->disableSelfCollisionCheck();
1343 skel->enableAdjacentBodyCheck();
1344 EXPECT_FALSE(skel->isEnabledSelfCollisionCheck());
1345 EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck());
1346 EXPECT_TRUE(group->collide());
1347 EXPECT_FALSE(group->collide(option));
1348
1349 skel->disableSelfCollisionCheck();
1350 skel->disableAdjacentBodyCheck();
1351 EXPECT_FALSE(skel->isEnabledSelfCollisionCheck());
1352 EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck());
1353 EXPECT_TRUE(group->collide());
1354 EXPECT_FALSE(group->collide(option));
1355
1356 // Test blacklist
1357 skel->enableSelfCollisionCheck();
1358 skel->enableAdjacentBodyCheck();
1359 bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
1360 EXPECT_FALSE(group->collide(option));
1361 bodyNodeFilter->removeBodyNodePairFromBlackList(body0, body1);
1362 EXPECT_TRUE(group->collide(option));
1363 bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
1364 EXPECT_FALSE(group->collide(option));
1365 bodyNodeFilter->removeAllBodyNodePairsFromBlackList();
1366 EXPECT_TRUE(group->collide(option));
1367 }
1368
1369 //==============================================================================
1370 TEST_F(Collision, Filter)
1371 {
1372 auto fcl_mesh_dart = FCLCollisionDetector::create();
1373 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1374 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
1375 testFilter(fcl_mesh_dart);
1376
1377 // auto fcl_prim_fcl = FCLCollisionDetector::create();
1378 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1379 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1380 // testFilter(fcl_prim_fcl);
1381
1382 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1383 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1384 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
1385 // testFilter(fcl_mesh_fcl);
1386
1387 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1388 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1389 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1390 // testFilter(fcl_mesh_fcl);
1391
1392 #if HAVE_BULLET
1393 auto bullet = BulletCollisionDetector::create();
1394 testFilter(bullet);
1395 #endif
1396
1397 auto dart = DARTCollisionDetector::create();
1398 testFilter(dart);
1399 }
1400
1401 //==============================================================================
1402 void testCreateCollisionGroups(const std::shared_ptr<CollisionDetector>& cd)
1403 {
1404 Eigen::Vector3d size(1.0, 1.0, 1.0);
1405 Eigen::Vector3d pos1(0.0, 0.0, 0.0);
1406 Eigen::Vector3d pos2(0.5, 0.0, 0.0);
1407
1408 auto boxSkeleton1 = createBox(size, pos1);
1409 auto boxSkeleton2 = createBox(size, pos2);
1410
1411 auto boxBodyNode1 = boxSkeleton1->getBodyNode(0u);
1412 auto boxBodyNode2 = boxSkeleton2->getBodyNode(0u);
1413
1414 auto boxShapeNode1 = boxBodyNode1->getShapeNodeWith<CollisionAspect>(0);
1415 auto boxShapeNode2 = boxBodyNode2->getShapeNodeWith<CollisionAspect>(0);
1416
1417 collision::CollisionOption option;
1418 collision::CollisionResult result;
1419
1420 auto skeletonGroup1 = cd->createCollisionGroup(boxSkeleton1.get());
1421 auto skeletonGroup2 = cd->createCollisionGroup(boxSkeleton2.get());
1422
1423 auto bodyNodeGroup1 = cd->createCollisionGroup(boxBodyNode1);
1424 auto bodyNodeGroup2 = cd->createCollisionGroup(boxBodyNode2);
1425
1426 auto shapeNodeGroup1 = cd->createCollisionGroup(boxShapeNode1);
1427 auto shapeNodeGroup2 = cd->createCollisionGroup(boxShapeNode2);
1428
1429 EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, &result));
1430 EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, &result));
1431 EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, &result));
1432
1433 // Binary check without passing option
1434 auto oldMaxNumContacts = option.maxNumContacts;
1435 option.maxNumContacts = 1u;
1436 EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option));
1437 EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option));
1438 EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option));
1439 option.maxNumContacts = oldMaxNumContacts;
1440
1441 // Binary check without passing option and result
1442 EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get()));
1443 EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get()));
1444 EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get()));
1445
1446 // Regression test for #666
1447 auto world = std::make_unique<World>();
1448 world->getConstraintSolver()->setCollisionDetector(cd);
1449 world->addSkeleton(boxSkeleton1);
1450 world->addSkeleton(boxSkeleton2);
1451 DART_SUPPRESS_DEPRECATED_BEGIN
1452 EXPECT_FALSE(boxBodyNode1->isColliding());
1453 EXPECT_FALSE(boxBodyNode2->isColliding());
1454 DART_SUPPRESS_DEPRECATED_END
1455
1456 const collision::CollisionResult& result1 = world->getLastCollisionResult();
1457 EXPECT_FALSE(result1.inCollision(boxBodyNode1));
1458 EXPECT_FALSE(result1.inCollision(boxBodyNode2));
1459
1460 world->step();
1461 DART_SUPPRESS_DEPRECATED_BEGIN
1462 EXPECT_TRUE(boxBodyNode1->isColliding());
1463 EXPECT_TRUE(boxBodyNode2->isColliding());
1464 DART_SUPPRESS_DEPRECATED_END
1465
1466 const collision::CollisionResult& result2 = world->getLastCollisionResult();
1467 EXPECT_TRUE(result2.inCollision(boxBodyNode1));
1468 EXPECT_TRUE(result2.inCollision(boxBodyNode2));
1469 }
1470
1471 //==============================================================================
1472 TEST_F(Collision, CreateCollisionGroupFromVariousObject)
1473 {
1474 auto fcl_mesh_dart = FCLCollisionDetector::create();
1475 fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1476 fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART);
1477 testCreateCollisionGroups(fcl_mesh_dart);
1478
1479 // auto fcl_prim_fcl = FCLCollisionDetector::create();
1480 // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
1481 // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1482 // testCreateCollisionGroups(fcl_prim_fcl);
1483
1484 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1485 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1486 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART);
1487 // testCreateCollisionGroups(fcl_mesh_fcl);
1488
1489 // auto fcl_mesh_fcl = FCLCollisionDetector::create();
1490 // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE);
1491 // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL);
1492 // testCreateCollisionGroups(fcl_mesh_fcl);
1493
1494 #if HAVE_BULLET
1495 auto bullet = BulletCollisionDetector::create();
1496 testCreateCollisionGroups(bullet);
1497 #endif
1498
1499 auto dart = DARTCollisionDetector::create();
1500 testCreateCollisionGroups(dart);
1501 }
1502
1503 //==============================================================================
1504 TEST_F(Collision, CollisionOfPrescribedJoints)
1505 {
1506 // There are one red plate (static skeleton) and 5 pendulums with different
1507 // actuator types. This test check if the motion prescribed joints are exactly
1508 // tracking the prescribed motion eventhough there are collision with other
1509 // objects.
1510
1511 const double tol = 1e-9;
1512 const double timeStep = 1e-3;
1513 const std::size_t numFrames = 5e+0; // 5 secs
1514
1515 // Load world and skeleton
1516 WorldPtr world = SkelParser::readWorld(
1517 "dart://sample/skel/test/collision_of_prescribed_joints_test.skel");
1518 world->setTimeStep(timeStep);
1519 EXPECT_TRUE(world != nullptr);
1520 EXPECT_NEAR(world->getTimeStep(), timeStep, tol);
1521
1522 SkeletonPtr skel1 = world->getSkeleton("skeleton 1");
1523 SkeletonPtr skel2 = world->getSkeleton("skeleton 2");
1524 SkeletonPtr skel3 = world->getSkeleton("skeleton 3");
1525 SkeletonPtr skel4 = world->getSkeleton("skeleton 4");
1526 SkeletonPtr skel5 = world->getSkeleton("skeleton 5");
1527 SkeletonPtr skel6 = world->getSkeleton("skeleton 6");
1528 EXPECT_TRUE(skel1 != nullptr);
1529 EXPECT_TRUE(skel2 != nullptr);
1530 EXPECT_TRUE(skel3 != nullptr);
1531 EXPECT_TRUE(skel4 != nullptr);
1532 EXPECT_TRUE(skel5 != nullptr);
1533 EXPECT_TRUE(skel6 != nullptr);
1534
1535 Joint* joint1 = skel1->getJoint(0);
1536 Joint* joint2 = skel2->getJoint(0);
1537 Joint* joint3 = skel3->getJoint(0);
1538 Joint* joint4 = skel4->getJoint(0);
1539 Joint* joint5 = skel5->getJoint(0);
1540 Joint* joint6 = skel6->getJoint(0);
1541 EXPECT_TRUE(joint1 != nullptr);
1542 EXPECT_TRUE(joint2 != nullptr);
1543 EXPECT_TRUE(joint3 != nullptr);
1544 EXPECT_TRUE(joint4 != nullptr);
1545 EXPECT_TRUE(joint5 != nullptr);
1546 EXPECT_TRUE(joint6 != nullptr);
1547 EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE);
1548 EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE);
1549 EXPECT_EQ(joint3->getActuatorType(), Joint::SERVO);
1550 EXPECT_EQ(joint4->getActuatorType(), Joint::ACCELERATION);
1551 EXPECT_EQ(joint5->getActuatorType(), Joint::VELOCITY);
1552 EXPECT_EQ(joint6->getActuatorType(), Joint::LOCKED);
1553
1554 for (std::size_t i = 0; i < numFrames; ++i)
1555 {
1556 const double time = world->getTime();
1557
1558 joint1->setCommand(0, -0.5 * constantsd::pi() * std::cos(time));
1559 joint2->setCommand(0, -0.5 * constantsd::pi() * std::cos(time));
1560 joint3->setCommand(0, -0.5 * constantsd::pi() * std::cos(time));
1561 joint4->setCommand(0, -0.5 * constantsd::pi() * std::cos(time));
1562 joint5->setCommand(0, -0.5 * constantsd::pi() * std::sin(time));
1563 joint6->setCommand(0, -0.5 * constantsd::pi() * std::sin(time)); // ignored
1564
1565 world->step(false);
1566
1567 EXPECT_TRUE(joint1->isDynamic());
1568 EXPECT_TRUE(joint2->isDynamic());
1569 EXPECT_TRUE(joint3->isDynamic());
1570
1571 // Check if the motion prescribed joints are following the prescribed motion
1572 // eventhough there is a collision with other objects
1573 EXPECT_TRUE(joint4->isKinematic());
1574 EXPECT_NEAR(joint4->getAcceleration(0), joint4->getCommand(0), tol);
1575 EXPECT_TRUE(joint5->isKinematic());
1576 EXPECT_NEAR(joint5->getVelocity(0), joint5->getCommand(0), tol);
1577
1578 // The velocity and acceleration of locked joint always must be zero.
1579 EXPECT_TRUE(joint6->isKinematic());
1580 EXPECT_NEAR(joint6->getVelocity(0), 0.0, tol);
1581 EXPECT_NEAR(joint6->getAcceleration(0), 0.0, tol);
1582 }
1583 }
1584
1585 //==============================================================================
1586 TEST_F(Collision, Factory)
1587 {
1588 EXPECT_TRUE(collision::CollisionDetector::getFactory()->canCreate("fcl"));
1589 EXPECT_TRUE(collision::CollisionDetector::getFactory()->canCreate("dart"));
1590
1591 #if HAVE_BULLET
1592 EXPECT_TRUE(collision::CollisionDetector::getFactory()->canCreate("bullet"));
1593 #else
1594 EXPECT_TRUE(!collision::CollisionDetector::getFactory()->canCreate("bullet"));
1595 #endif
1596
1597 #if HAVE_ODE
1598 EXPECT_TRUE(collision::CollisionDetector::getFactory()->canCreate("ode"));
1599 #else
1600 EXPECT_TRUE(!collision::CollisionDetector::getFactory()->canCreate("ode"));
1601 #endif
1602 }
1603
1604 //==============================================================================
1605 #if HAVE_OCTOMAP && FCL_HAVE_OCTOMAP
1606 TEST_F(Collision, VoxelGrid)
1607 {
1608 auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
1609 auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
1610
1611 auto shape1 = std::make_shared<VoxelGridShape>(0.01);
1612 auto shape2 = std::make_shared<SphereShape>(0.001);
1613
1614 simpleFrame1->setShape(shape1);
1615 simpleFrame2->setShape(shape2);
1616
1617 auto cd = FCLCollisionDetector::create();
1618 auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());
1619
1620 EXPECT_EQ(group->getNumShapeFrames(), 2u);
1621
1622 collision::CollisionOption option;
1623 option.enableContact = true;
1624
1625 collision::CollisionResult result;
1626
1627 result.clear();
1628 simpleFrame2->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
1629 EXPECT_FALSE(group->collide(option, &result));
1630 EXPECT_TRUE(result.getNumContacts() == 0u);
1631
1632 result.clear();
1633 shape1->updateOccupancy(Eigen::Vector3d(0.0, 0.0, 0.0), true);
1634 simpleFrame2->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
1635 EXPECT_TRUE(group->collide(option, &result));
1636 EXPECT_TRUE(result.getNumContacts() >= 1u);
1637 }
1638 #endif // HAVE_OCTOMAP && FCL_HAVE_OCTOMAP