release-6.4 --> master (#938)

dartsim · Nov 11, 2017 · 6d8058d · 6d8058d
1 parent 12894a2
commit 6d8058d
Show file tree

Hide file tree

Showing 42 changed files with 347 additions and 164 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -14,13 +14,22 @@
 
 ### DART 6.4.0 (201X-XX-XX)
 
-* Kinematics
+* Kinematics/Dynamics
 
   * Added IkFast support as analytic IK solver: [#887](https://github.com/dartsim/dart/pull/887)
+  * Fixed NaN values caused by zero-length normals in ContactConstraint: [#881](https://github.com/dartsim/dart/pull/881)
+
+* Collision detection
+
+  * Added FCL 0.6 support (backport of #873): [#936](https://github.com/dartsim/dart/pull/936)
 
 * License
 
   * Added Personal Robotics Lab and Open Source Robotics Foundation as contributors: [#929](https://github.com/dartsim/dart/pull/929)
+
+* Misc
+
+  * Suppressed warnings: [#937](https://github.com/dartsim/dart/pull/937)
 
 ### [DART 6.3.0 (2017-10-04)](https://github.com/dartsim/dart/milestone/36?closed=1)
 

diff --git a/dart/collision/Contact.cpp b/dart/collision/Contact.cpp
@@ -51,5 +51,20 @@ Contact::Contact()
   // Do nothing
 }
 
+//==============================================================================
+bool Contact::isZeroNormal(const Eigen::Vector3d& normal)
+{
+  if (normal.squaredNorm() < getNormalEpsilonSquared())
+    return true;
+  else
+    return false;
+}
+
+//==============================================================================
+bool Contact::isNonZeroNormal(const Eigen::Vector3d& normal)
+{
+  return !isZeroNormal(normal);
+}
+
 }  // namespace collision
 }  // namespace dart
diff --git a/dart/collision/Contact.hpp b/dart/collision/Contact.hpp
@@ -78,9 +78,24 @@ struct Contact
   // TODO(JS): userData is an experimental variable.
   /// \brief User data.
   void* userData;
+
+  /// Returns the epsilon to be used for determination of zero-length normal.
+  constexpr static double getNormalEpsilon();
+
+  /// Returns the squired epsilon to be used for determination of zero-length
+  /// normal.
+  constexpr static double getNormalEpsilonSquared();
+
+  /// Returns true if the length of a normal is less than the epsilon.
+  static bool isZeroNormal(const Eigen::Vector3d& normal);
+
+  /// Returns !isZeroNormal().
+  static bool isNonZeroNormal(const Eigen::Vector3d& normal);
 };
 
 }  // namespace collision
 }  // namespace dart
 
+#include "dart/collision/detail/Contact-impl.hpp"
+
 #endif  // DART_COLLISION_CONTACT_HPP_
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -594,6 +594,14 @@ void reportContacts(
     for (auto j = 0; j < numContacts; ++j)
     {
       const auto& cp = contactManifold->getContactPoint(j);
+
+      if (cp.m_normalWorldOnB.length2() < Contact::getNormalEpsilonSquared())
+      {
+        // Skip this contact. This is because we assume that a contact with
+        // zero-length normal is invalid.
+        continue;
+      }
+
       result.addContact(convertContact(cp, collObj0, collObj1));
 
       // No need to check further collisions

diff --git a/dart/collision/detail/Contact-impl.hpp b/dart/collision/detail/Contact-impl.hpp
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2011-2017, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_DETAIL_CONTACT_IMPL_HPP_
+#define DART_COLLISION_DETAIL_CONTACT_IMPL_HPP_
+
+#include "dart/collision/Contact.hpp"
+
+namespace dart {
+namespace collision {
+
+//==============================================================================
+constexpr double Contact::getNormalEpsilon()
+{
+  return 1e-6;
+}
+
+//==============================================================================
+constexpr double Contact::getNormalEpsilonSquared()
+{
+  return 1e-12;
+}
+
+}  // namespace collision
+}  // namespace dart
+
+#endif // DART_COLLISION_DETAIL_CONTACT_IMPL_HPP_
diff --git a/dart/collision/fcl/BackwardCompatibility.cpp b/dart/collision/fcl/BackwardCompatibility.cpp
@@ -46,6 +46,16 @@ double length(const dart::collision::fcl::Vector3& t)
 #endif
 }
 
+//==============================================================================
+double length2(const dart::collision::fcl::Vector3& t)
+{
+#if FCL_VERSION_AT_LEAST(0,6,0)
+  return t.squaredNorm();
+#else
+  return t.sqrLength();
+#endif
+}
+
 //==============================================================================
 dart::collision::fcl::Vector3 getTranslation(
     const dart::collision::fcl::Transform3& T)

diff --git a/dart/collision/fcl/BackwardCompatibility.hpp b/dart/collision/fcl/BackwardCompatibility.hpp
@@ -135,6 +135,9 @@ using Ellipsoid = ::fcl::Ellipsoid;
 /// Returns norm of a 3-dim vector
 double length(const dart::collision::fcl::Vector3& t);
 
+/// Returns squared norm of a 3-dim vector
+double length2(const dart::collision::fcl::Vector3& t);
+
 /// Returns translation component of a transform
 dart::collision::fcl::Vector3 getTranslation(
     const dart::collision::fcl::Transform3& T);

diff --git a/dart/collision/fcl/CollisionShapes.hpp b/dart/collision/fcl/CollisionShapes.hpp
@@ -33,10 +33,9 @@
 #ifndef DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
 #define DART_COLLISION_FCL_MESH_COLLISIONSHAPES_HPP_
 
-#include "dart/collision/fcl/FCLTypes.hpp"
-
 #include <cmath>
 #include <assimp/scene.h>
+#include "dart/collision/fcl/BackwardCompatibility.hpp"
 
 namespace dart {
 namespace collision {
@@ -57,8 +56,8 @@ ::fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,
             = _mesh->mMeshes[i]->mVertices[
                   _mesh->mMeshes[i]->mFaces[j].mIndices[k]];
         vertices[k] = dart::collision::fcl::Vector3(vertex.x * _scaleX,
-                                          vertex.y * _scaleY,
-                                          vertex.z * _scaleZ);
+                                                    vertex.y * _scaleY,
+                                                    vertex.z * _scaleZ);
         vertices[k] = dart::collision::fcl::transform(_transform, vertices[k]);
       }
       model->addTriangle(vertices[0], vertices[1], vertices[2]);
@@ -254,14 +253,14 @@ ::fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ,
 
   for (int i = 0; i < 112; i++) {
     p1 = dart::collision::fcl::Vector3(v[f[i][0]][0] * _sizeX,
-                            v[f[i][0]][1] * _sizeY,
-                            v[f[i][0]][2] * _sizeZ);
+                                       v[f[i][0]][1] * _sizeY,
+                                       v[f[i][0]][2] * _sizeZ);
     p2 = dart::collision::fcl::Vector3(v[f[i][1]][0] * _sizeX,
-                            v[f[i][1]][1] * _sizeY,
-                            v[f[i][1]][2] * _sizeZ);
+                                       v[f[i][1]][1] * _sizeY,
+                                       v[f[i][1]][2] * _sizeZ);
     p3 = dart::collision::fcl::Vector3(v[f[i][2]][0] * _sizeX,
-                            v[f[i][2]][1] * _sizeY,
-                            v[f[i][2]][2] * _sizeZ);
+                                       v[f[i][2]][1] * _sizeY,
+                                       v[f[i][2]][2] * _sizeZ);
     p1 = dart::collision::fcl::transform(_transform, p1);
     p2 = dart::collision::fcl::transform(_transform, p2);
     p3 = dart::collision::fcl::transform(_transform, p3);
@@ -395,14 +394,14 @@ ::fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
       radiusHigh = _baseRadius
                    - deltaRadius * (static_cast<float>(j + 1) / _stacks);
 
-      p1 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i],
-                              zLow);
-      p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1],
-                              zLow);
-      p3 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i], radiusHigh * cosCache[i],
-                              zHigh);
-      p4 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1],
-                              zHigh);
+      p1 = dart::collision::fcl::Vector3(
+          radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
+      p2 = dart::collision::fcl::Vector3(
+          radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);
+      p3 = dart::collision::fcl::Vector3(
+          radiusHigh * sinCache[i], radiusHigh * cosCache[i], zHigh);
+      p4 = dart::collision::fcl::Vector3(
+          radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], zHigh);
       p1 = dart::collision::fcl::transform(_transform, p1);
       p2 = dart::collision::fcl::transform(_transform, p2);
       p3 = dart::collision::fcl::transform(_transform, p3);

diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp
@@ -483,7 +483,7 @@ ::fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ)
 //==============================================================================
 template<class BV>
 ::fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
-                                        double _height, int _slices, int _stacks)
+                                    double _height, int _slices, int _stacks)
 {
   const int CACHE_SIZE = 240;
 
@@ -552,14 +552,14 @@ ::fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
       radiusHigh = _baseRadius
                    - deltaRadius * (static_cast<float>(j + 1) / _stacks);
 
-      p1 = dart::collision::fcl::Vector3(radiusLow * sinCache[i], radiusLow * cosCache[i],
-                              zLow);
-      p2 = dart::collision::fcl::Vector3(radiusLow * sinCache[i+1], radiusLow * cosCache[i+1],
-                              zLow);
-      p3 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i], radiusHigh * cosCache[i],
-                              zHigh);
-      p4 = dart::collision::fcl::Vector3(radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1],
-                              zHigh);
+      p1 = dart::collision::fcl::Vector3(
+          radiusLow * sinCache[i], radiusLow * cosCache[i], zLow);
+      p2 = dart::collision::fcl::Vector3(
+          radiusLow * sinCache[i+1], radiusLow * cosCache[i+1], zLow);
+      p3 = dart::collision::fcl::Vector3(
+          radiusHigh * sinCache[i], radiusHigh * cosCache[i], zHigh);
+      p4 = dart::collision::fcl::Vector3(
+          radiusHigh * sinCache[i+1], radiusHigh * cosCache[i+1], zHigh);
 
       model->addTriangle(p1, p2, p3);
       model->addTriangle(p2, p3, p4);
@@ -585,8 +585,8 @@ ::fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
 
 //==============================================================================
 template<class BV>
-::fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,
-                                    const aiScene* _mesh)
+::fcl::BVHModel<BV>* createMesh(
+    float _scaleX, float _scaleY, float _scaleZ, const aiScene* _mesh)
 {
   // Create FCL mesh from Assimp mesh
 
@@ -604,8 +604,8 @@ ::fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,
             = _mesh->mMeshes[i]->mVertices[
               _mesh->mMeshes[i]->mFaces[j].mIndices[k]];
         vertices[k] = dart::collision::fcl::Vector3(vertex.x * _scaleX,
-                                         vertex.y * _scaleY,
-                                         vertex.z * _scaleZ);
+                                                    vertex.y * _scaleY,
+                                                    vertex.z * _scaleZ);
       }
       model->addTriangle(vertices[0], vertices[1], vertices[2]);
     }
@@ -1290,8 +1290,20 @@ void postProcessFCL(
   // without the checkings of repeatidity and co-linearity.
   if (1u == option.maxNumContacts)
   {
-    result.addContact(convertContact(fclResult.getContact(0), o1, o2, option));
+    for (auto i = 0u; i < numContacts; ++i)
+    {
+      if (dart::collision::fcl::length2(fclResult.getContact(i).normal)
+          < Contact::getNormalEpsilonSquared())
+      {
+        // Skip this contact. This is because we assume that a contact with
+        // zero-length normal is invalid.
+        continue;
+      }
 
+      result.addContact(
+          convertContact(fclResult.getContact(i), o1, o2, option));
+      break;
+    }
     return;
   }
 
@@ -1317,6 +1329,14 @@ void postProcessFCL(
     if (markForDeletion[i])
       continue;
 
+    if (dart::collision::fcl::length2(fclResult.getContact(i).normal)
+        < Contact::getNormalEpsilonSquared())
+    {
+      // Skip this contact. This is because we assume that a contact with
+      // zero-length normal is invalid.
+      continue;
+    }
+
     result.addContact(convertContact(fclResult.getContact(i), o1, o2, option));
 
     if (result.getNumContacts() >= option.maxNumContacts)
@@ -1356,6 +1376,12 @@ void postProcessDART(
     if (option.enableContact)
     {
       pair1.normal = FCLTypes::convertVector3(-c.normal);
+      if (Contact::isZeroNormal(pair1.normal))
+      {
+        // This is an invalid contact, as it contains a zero length normal.
+        // Skip this contact.
+        continue;
+      }
       pair1.penetrationDepth = c.penetration_depth;
       pair1.triID1 = c.b1;
       pair1.triID2 = c.b2;

diff --git a/dart/collision/fcl/FCLCollisionGroup.hpp b/dart/collision/fcl/FCLCollisionGroup.hpp
@@ -34,7 +34,7 @@
 #define DART_COLLISION_FCL_FCLCOLLISIONGROUP_HPP_
 
 #include "dart/collision/CollisionGroup.hpp"
-#include "dart/collision/fcl/FCLTypes.hpp"
+#include "dart/collision/fcl/BackwardCompatibility.hpp"
 
 namespace dart {
 namespace collision {

diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp
@@ -34,7 +34,7 @@
 #define DART_COLLISION_FCL_FCLCOLLISIONOBJECT_HPP_
 
 #include "dart/collision/CollisionObject.hpp"
-#include "dart/collision/fcl/FCLTypes.hpp"
+#include "dart/collision/fcl/BackwardCompatibility.hpp"
 
 namespace dart {
 namespace collision {