Merge pull request #167 from dartsim/isnan

Add portable isNan() and isInf()
dartsim · Mar 27, 2014 · c1f96f7 · c1f96f7
2 parents 3c1e109 + eed78fc
commit c1f96f7
Show file tree

Hide file tree

Showing 17 changed files with 86 additions and 34 deletions.
diff --git a/Migration.md b/Migration.md
@@ -2,6 +2,12 @@
 
 ### Additions
 
+1. **dart/math/Helpers.h**
+  + bool isNan(double _v)
+  + bool isNan(const Eigen::MatrixXd& _m)
+  + bool isInf(double _v)
+  + bool isInf(const Eigen::MatrixXd& _m)
+
 1. **dart/dynamics/BodyNode.h**
   + enum InverseKinematicsPolicy
   + class TransformObjFunc
@@ -31,6 +37,10 @@
 1. **dart/dynamics/Skeleton.h**
   + Eigen::VectorXd getConfig() const
 
+1. **dart/math/Geometry.h**
+  + bool isNan(const Eigen::MatrixXd& _m)
+    + Note: moved to dart/math/Helpers.h
+
 ### Modifications
 
 1. **dart/dynamics/Skeleton.h**

diff --git a/apps/balance/Main.cpp b/apps/balance/Main.cpp
@@ -35,6 +35,7 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <iostream>
 #include <vector>
 
 #include "dart/utils/Paths.h"

diff --git a/apps/harnessTest/Main.cpp b/apps/harnessTest/Main.cpp
@@ -36,6 +36,8 @@
 
 #include "apps/harnessTest/MyWindow.h"
 
+#include <iostream>
+
 #include "dart/utils/Paths.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/simulation/World.h"

diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp
@@ -38,6 +38,7 @@
 #include "dart/collision/CollisionDetector.h"
 
 #include <algorithm>
+#include <iostream>
 #include <vector>
 
 #include "dart/dynamics/BodyNode.h"

diff --git a/dart/collision/bullet/BulletCollisionNode.cpp b/dart/collision/bullet/BulletCollisionNode.cpp
@@ -36,6 +36,8 @@
 
 #include "dart/collision/bullet/BulletCollisionNode.h"
 
+#include <iostream>
+
 #include <assimp/scene.h>
 
 #include "dart/collision/bullet/BulletTypes.h"

diff --git a/dart/dynamics/GenCoord.cpp b/dart/dynamics/GenCoord.cpp
@@ -38,6 +38,7 @@
 #include <cassert>
 
 #include "dart/math/MathTypes.h"
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/GenCoord.h"
 
 namespace dart {
@@ -146,7 +147,7 @@ double GenCoord::getConfigMax() const
 //==============================================================================
 void GenCoord::setConfigDeriv(double _configDeriv)
 {
-  assert(_configDeriv == _configDeriv);
+  assert(!math::isNan(_configDeriv));
   mConfigDeriv = _configDeriv;
 }
 
@@ -165,7 +166,7 @@ double GenCoord::getVelMax() const
 //==============================================================================
 void GenCoord::setVelDeriv(double _velDeriv)
 {
-  assert(_velDeriv == _velDeriv);
+  assert(!math::isNan(_velDeriv));
   mVelDeriv = _velDeriv;
 }
 
@@ -184,7 +185,7 @@ double GenCoord::getAccMax() const
 //==============================================================================
 void GenCoord::setAccDeriv(double _accDeriv)
 {
-  assert(_accDeriv == _accDeriv);
+  assert(!math::isNan(_accDeriv));
   mAccDeriv = _accDeriv;
 }
 
@@ -203,7 +204,7 @@ double GenCoord::getForceMax() const
 //==============================================================================
 void GenCoord::setForceDeriv(double _forceDeriv)
 {
-  assert(_forceDeriv == _forceDeriv);
+  assert(!math::isNan(_forceDeriv));
   mForceDeriv = _forceDeriv;
 }
 
@@ -216,28 +217,28 @@ double GenCoord::getForceDeriv() const
 //==============================================================================
 void GenCoord::setConfig(double _config)
 {
-  assert(_config == _config);
+  assert(!math::isNan(_config));
   mConfig = _config;
 }
 
 //==============================================================================
 void GenCoord::setVel(double _vel)
 {
-  assert(_vel == _vel);
+  assert(!math::isNan(_vel));
   mVel = _vel;
 }
 
 //==============================================================================
 void GenCoord::setAcc(double _acc)
 {
-  assert(_acc == _acc);
+  assert(!math::isNan(_acc));
   mAcc = _acc;
 }
 
 //==============================================================================
 void GenCoord::setForce(double _force)
 {
-  assert(_force == _force);
+  assert(!math::isNan(_force));
   mForce = _force;
 }
 

diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp
@@ -40,6 +40,7 @@
 
 #include "dart/common/Console.h"
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 
 namespace dart {
 namespace dynamics {

diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp
@@ -335,16 +335,16 @@ void PointMass::updateArticulatedInertia(double _dt)
       = 1.0 / (mMass
                + _dt * mParentSoftBodyNode->getDampingCoefficient()
                + _dt * _dt * mParentSoftBodyNode->getVertexSpringStiffness());
-  assert(!std::isnan(mImplicitPsi));
+  assert(!math::isNan(mImplicitPsi));
 
   // Cache data: AI_S_Psi
   // - Do nothing
 
   // Cache data: Pi
   mPi         = mMass - mMass * mMass * mPsi;
   mImplicitPi = mMass - mMass * mMass * mImplicitPsi;
-  assert(!std::isnan(mPi));
-  assert(!std::isnan(mImplicitPi));
+  assert(!math::isNan(mPi));
+  assert(!math::isNan(mImplicitPi));
 }
 
 void PointMass::updateGeneralizedForce(bool _withDampingForces)

diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp
@@ -39,6 +39,7 @@
 #include <string>
 
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 
 namespace dart {

diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp
@@ -40,6 +40,7 @@
 
 #include "dart/common/Console.h"
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 
 namespace dart {

diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp
@@ -39,6 +39,7 @@
 #include <string>
 
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 #include "dart/dynamics/BodyNode.h"
 
 namespace dart {

diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp
@@ -39,6 +39,7 @@
 #include <string>
 #include <vector>
 
+#include "dart/math/Helpers.h"
 #include <dart/common/Console.h>
 #include <dart/dynamics/Joint.h>
 #include <dart/dynamics/Shape.h>

diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp
@@ -39,6 +39,7 @@
 #include <string>
 
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 
 namespace dart {
 namespace dynamics {

diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * Copyright (c) 2011-2014, Georgia Tech Research Corporation
  * All rights reserved.
  *
  * Author(s): Sehoon Ha <sehoon.ha@gmail.com>,
@@ -36,9 +36,14 @@
  */
 
 #include <algorithm>
+#include <cassert>
+#include <cmath>
 #include <iomanip>
+#include <iostream>
+#include <vector>
 
 #include "dart/math/Geometry.h"
+#include "dart/math/Helpers.h"
 
 namespace dart {
 namespace math {
@@ -1426,15 +1431,6 @@ bool verifyTransform(const Eigen::Isometry3d& _T) {
       && fabs(_T.linear().determinant() - 1.0) <= DART_EPSILON;
 }
 
-bool isNan(const Eigen::MatrixXd& _m) {
-  for (int i = 0; i < _m.rows(); ++i)
-    for (int j = 0; j < _m.cols(); ++j)
-      if (_m(i, j) != _m(i, j))
-        return true;
-
-  return false;
-}
-
 Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m) {
 #ifndef NDEBUG
   if (fabs(_m(0, 0)) > DART_EPSILON

diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * Copyright (c) 2011-2014, Georgia Tech Research Corporation
  * All rights reserved.
  *
  * Author(s): Sehoon Ha <sehoon.ha@gmail.com>,
@@ -38,12 +38,6 @@
 #ifndef DART_MATH_GEOMETRY_H_
 #define DART_MATH_GEOMETRY_H_
 
-#include <vector>
-#include <cassert>
-#include <iostream>
-#include <cmath>
-#include <cfloat>
-
 #include <Eigen/Dense>
 
 #include "dart/math/MathTypes.h"
@@ -287,9 +281,6 @@ bool verifyRotation(const Eigen::Matrix3d& _R);
 /// all the elements are not NaN values.
 bool verifyTransform(const Eigen::Isometry3d& _T);
 
-/// \brief
-bool isNan(const Eigen::MatrixXd& _m);
-
 }  // namespace math
 }  // namespace dart
 

diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2011-2013, Georgia Tech Research Corporation
+ * Copyright (c) 2011-2014, Georgia Tech Research Corporation
  * All rights reserved.
  *
  * Author(s): Sehoon Ha <sehoon.ha@gmail.com>
@@ -39,6 +39,7 @@
 
 // Standard Libraries
 #include <vector>
+#include <cfloat>
 #include <cmath>
 #include <ctime>
 #include <climits>
@@ -132,6 +133,46 @@ inline bool isInt(double _x) {
   return false;
 }
 
+/// \brief Returns whether _v is a NaN (Not-A-Number) value
+inline bool isNan(double _v) {
+#ifdef WIN32
+  return _isnan(_v);
+#else
+  return std::isnan(_v);
+#endif
+}
+
+/// \brief Returns whether _m is a NaN (Not-A-Number) matrix
+inline bool isNan(const Eigen::MatrixXd& _m) {
+  for (int i = 0; i < _m.rows(); ++i)
+    for (int j = 0; j < _m.cols(); ++j)
+      if (isNan(_m(i, j)))
+        return true;
+
+  return false;
+}
+
+/// \brief Returns whether _v is an infinity value (either positive infinity or
+/// negative infinity).
+inline bool isInf(double _v) {
+#ifdef WIN32
+  return !_finite(_v);
+#else
+  return std::isinf(_v);
+#endif
+}
+
+/// \brief Returns whether _m is an infinity matrix (either positive infinity or
+/// negative infinity).
+inline bool isInf(const Eigen::MatrixXd& _m) {
+  for (int i = 0; i < _m.rows(); ++i)
+    for (int j = 0; j < _m.cols(); ++j)
+      if (isInf(_m(i, j)))
+        return true;
+
+  return false;
+}
+
 inline unsigned seedRand() {
   time_t now = time(0);
   unsigned char* p = reinterpret_cast<unsigned char*>(&now);

diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp
@@ -853,10 +853,11 @@ TEST_F(DynamicsTest, compareEquationsOfMotion)
   for (int i = 0; i < getList().size(); ++i)
   {
     ////////////////////////////////////////////////////////////////////////////
-    // TODO(JS): Following five skel files, which contain euler joints couldn't
+    // TODO(JS): Following skel files, which contain euler joints couldn't
     //           pass EQUATIONS_OF_MOTION, are disabled.
     std::string skelFileName = getList()[i];
-    if (skelFileName == DART_DATA_PATH"skel/test/chainwhipa.skel"
+    if (skelFileName == DART_DATA_PATH"skel/test/double_pendulum_euler_joint.skel"
+        || skelFileName == DART_DATA_PATH"skel/test/chainwhipa.skel"
         || skelFileName == DART_DATA_PATH"skel/test/serial_chain_eulerxyz_joint.skel"
         || skelFileName == DART_DATA_PATH"skel/test/simple_tree_structure_euler_joint.skel"
         || skelFileName == DART_DATA_PATH"skel/test/tree_structure_euler_joint.skel"