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