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