Updated RBDL to latest dev commit 73b7048.

3rdparty/rbdl/CMakeLists.txt

@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 3.13)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
 
 SET ( RBDL_VERSION_MAJOR 2 )
 SET ( RBDL_VERSION_MINOR 6 )

3rdparty/rbdl/README.md

@@ -142,6 +142,13 @@ be used by enabling `RBDL_USE_SIMPLE_MATH`, i.e.:
 
     cmake -D RBDL_USE_SIMPLE_MATH=TRUE ../
 
+VCPKG package manager (for Windows, Linux and Mac)
+==================================================
+
+Install vcpkg by making a local clone from its GitHub repo [https://github.com/Microsoft/vcpkg](https://github.com/Microsoft/vcpkg). Then run the vcpkg-bootstrapper script to set it up. For detailed installation instructions, see [Install vcpkg](https://docs.microsoft.com/en-us/cpp/build/install-vcpkg). To integrate vcpkg with your Visual Studio or Visual Studio Code development environment, see [Integrate vcpkg](https://docs.microsoft.com/en-us/cpp/build/integrate-vcpkg). Then, to use vcpkg to install or update a library, see [Manage libraries with vcpkg](https://docs.microsoft.com/en-us/cpp/build/manage-libraries-with-vcpkg). For more information about vcpkg commands, see [vcpkg command-line reference](https://docs.microsoft.com/en-us/cpp/build/vcpkg-command-line-reference).
+
+👀 RBDL is available in VCPKG since [2020-11 release](https://github.com/microsoft/vcpkg/releases/tag/2020.11)
+
 Python Bindings
 ===============
 

3rdparty/rbdl/addons/luamodel/luamodel.cc

@@ -13,6 +13,8 @@ extern "C"
 #include <lualib.h>
 }
 
+
+
 using namespace std;
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;

3rdparty/rbdl/include/rbdl/Dynamics.h

@@ -33,7 +33,7 @@ struct Model;
  *
  * This function computes the generalized forces from given generalized
  * states, velocities, and accelerations:
- *   \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$
+ *   \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}, f_\textit{ext}) \f$
  *
  * \param model rigid body model
  * \param Q     state vector of the internal joints
@@ -55,7 +55,7 @@ RBDL_DLLAPI void InverseDynamics (
  *
  * This function computes the generalized forces from given generalized
  * states, velocities, and accelerations:
- *   \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$
+ *   \f$ \tau = N(q, \dot{q}, f_\textit{ext}) \f$
  *
  * \param model rigid body model
  * \param Q     state vector of the internal joints

3rdparty/rbdl/include/rbdl/Quaternion.h

@@ -66,27 +66,34 @@ class Quaternion : public Vector4d {
           );
     }
 
-    Quaternion slerp (double alpha, const Quaternion &quat) const {
+    Quaternion slerp (double alpha, Quaternion quat) const {
       // check whether one of the two has 0 length
-      double s = std::sqrt (squaredNorm() * quat.squaredNorm());
+      double cos_half_theta = this->dot(quat);
-
+      if (fabs(cos_half_theta >= 1.0)) {
-      // division by 0.f is unhealthy!
-      assert (s != 0.);
-
-      double angle = acos (dot(quat) / s);
-      if (angle == 0. || std::isnan(angle)) {
         return *this;
       }
-      assert(!std::isnan(angle));
 
-      double d = 1. / std::sin (angle);
+      double half_theta = acos(cos_half_theta);
-      double p0 = std::sin ((1. - alpha) * angle);
+      double sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta);
-      double p1 = std::sin (alpha * angle);
 
-      if (dot (quat) < 0.) {
+      if (fabs(sin_half_theta) < 0.00001) {
-        return Quaternion( ((*this) * p0 - quat * p1) * d);
+        return Quaternion (
+            ((*this)[0] * 0.5 + quat[0] * 0.5),
+            ((*this)[1] * 0.5 + quat[1] * 0.5),
+            ((*this)[2] * 0.5 + quat[2] * 0.5),
+            ((*this)[3] * 0.5 + quat[3] * 0.5)
+            );
       }
-      return Quaternion( ((*this) * p0 + quat * p1) * d);
+
+      double ratio_a = sin((1 - alpha) * half_theta) / sin_half_theta;
+      double ratio_b = sin(alpha * half_theta) / sin_half_theta;
+
+      return Quaternion (
+          ((*this)[0] * ratio_a + quat[0] * ratio_b),
+          ((*this)[1] * ratio_a + quat[1] * ratio_b),
+          ((*this)[2] * ratio_a + quat[2] * ratio_b),
+          ((*this)[3] * ratio_a + quat[3] * ratio_b)
+      );
     }
 
     static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) {
@@ -101,12 +108,39 @@ class Quaternion : public Vector4d {
     }
 
     static Quaternion fromMatrix (const Matrix3d &mat) {
-      double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
+      float tr = mat(0,0) + mat(1,1) + mat(2,2);
+      if (tr > 0) {
+        float w = sqrt (1.f + tr) * 0.5;
         return Quaternion (
             (mat(1,2) - mat(2,1)) / (w * 4.),
             (mat(2,0) - mat(0,2)) / (w * 4.),
             (mat(0,1) - mat(1,0)) / (w * 4.),
             w);
+      } else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
+        float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
+        return Quaternion(
+            x,
+            (mat(1,0) + mat(0,1)) / (x * 4.),
+            (mat(2,0) + mat(0,2)) / (x * 4.),
+            (mat(1,2) - mat(2,1)) / (x * 4.)
+        );
+      } else if (mat(1,1) > mat(2,2)) {
+        float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
+        return Quaternion(
+            (mat(1,0) + mat(0,1)) / (y * 4.),
+            y,
+            (mat(2,1) + mat(1,2)) / (y * 4.),
+            (mat(2,0) - mat(0,2)) / (y * 4.)
+        );
+      } else {
+        float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
+        return Quaternion(
+            (mat(2,0) + mat(0,2)) / (z * 4.),
+            (mat(2,1) + mat(1,2)) / (z * 4.),
+            z,
+            (mat(0,1) - mat(1,0)) / (z * 4.)
+            );
+      }
     }
 
     static Quaternion fromZYXAngles (const Vector3d &zyx_angles) {

3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h

@@ -819,19 +819,16 @@ struct Storage {
 
   inline size_t cols() const { return NumCols; }
 
+  void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
+  // Resizing of fixed size matrices not allowed
   #ifndef NDEBUG
-  void resize(int num_rows, int num_cols) {
       if (num_rows != NumRows || num_cols != NumCols) {
         std::cout << "Error: trying to resize fixed matrix from "
           << NumRows << ", " << NumCols << " to "
           << num_rows << ", " << num_cols << "." << std::endl;
       }
-    assert (num_rows == NumRows && num_cols == NumCols);
-#else
-    void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
   #endif
-    // Resizing of fixed size matrices not allowed
+      assert (num_rows == NumRows && num_cols == NumCols);
-
   }
 
   inline ScalarType& coeff(int row_index, int col_index) {
@@ -1555,6 +1552,7 @@ private:
     typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
 
     bool mIsFactorized;
+    Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> mQ;
     Derived mL;
 
 public:
@@ -2349,25 +2347,32 @@ class Quaternion : public Vector4f {
 
     Quaternion slerp (float alpha, const Quaternion &quat) const {
       // check whether one of the two has 0 length
-      float s = sqrt (squaredNorm() * quat.squaredNorm());
+      double cos_half_theta = this->dot(quat);
-
+      if (fabs(cos_half_theta >= 1.0)) {
-      // division by 0.f is unhealthy!
-      assert (s != 0.f);
-
-      float angle = acos (dot(quat) / s);
-      if (angle == 0.f || std::isnan(angle)) {
         return *this;
       }
-      assert(!std::isnan(angle));
 
-      float d = 1.f / sinf (angle);
+      double half_theta = acos(cos_half_theta);
-      float p0 = sinf ((1.f - alpha) * angle);
+      double sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta);
-      float p1 = sinf (alpha * angle);
 
-      if (dot (quat) < 0.f) {
+      if (fabs(sin_half_theta) < 0.00001) {
-        return Quaternion( ((*this) * p0 - quat * p1) * d);
+        return Quaternion (
+            ((*this)[0] * 0.5 + quat[0] * 0.5),
+            ((*this)[1] * 0.5 + quat[1] * 0.5),
+            ((*this)[2] * 0.5 + quat[2] * 0.5),
+            ((*this)[3] * 0.5 + quat[3] * 0.5)
+        );
       }
-      return Quaternion( ((*this) * p0 + quat * p1) * d);
+
+      double ratio_a = sin((1 - alpha) * half_theta) / sin_half_theta;
+      double ratio_b = sin(alpha * half_theta) / sin_half_theta;
+
+      return Quaternion (
+          ((*this)[0] * ratio_a + quat[0] * ratio_b),
+          ((*this)[1] * ratio_a + quat[1] * ratio_b),
+          ((*this)[2] * ratio_a + quat[2] * ratio_b),
+          ((*this)[3] * ratio_a + quat[3] * ratio_b)
+      );
     }
 
     Matrix44f toGLMatrix() const {
@@ -2407,12 +2412,75 @@ class Quaternion : public Vector4f {
     }
 
     static Quaternion fromMatrix (const Matrix33f &mat) {
-      float w = sqrt (1.f + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5f;
+      float tr = mat(0,0) + mat(1,1) + mat(2,2);
+      if (tr > 0) {
+        float w = sqrt (1.f + tr) * 0.5;
         return Quaternion (
-          (mat(2,1) - mat(1,2)) / (w * 4.f),
+            (mat(1,2) - mat(2,1)) / (w * 4.),
-          (mat(0,2) - mat(2,0)) / (w * 4.f),
+            (mat(2,0) - mat(0,2)) / (w * 4.),
-          (mat(1,0) - mat(0,1)) / (w * 4.f),
+            (mat(0,1) - mat(1,0)) / (w * 4.),
             w);
+      } else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
+        float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
+        return Quaternion(
+            x,
+            (mat(1,0) + mat(0,1)) / (x * 4.),
+            (mat(2,0) + mat(0,2)) / (x * 4.),
+            (mat(1,2) - mat(2,1)) / (x * 4.)
+        );
+      } else if (mat(1,1) > mat(2,2)) {
+        float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
+        return Quaternion(
+            (mat(1,0) + mat(0,1)) / (y * 4.),
+            y,
+            (mat(2,1) + mat(1,2)) / (y * 4.),
+            (mat(2,0) - mat(0,2)) / (y * 4.)
+        );
+      } else {
+        float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
+        return Quaternion(
+            (mat(2,0) + mat(0,2)) / (z * 4.),
+            (mat(2,1) + mat(1,2)) / (z * 4.),
+            z,
+            (mat(0,1) - mat(1,0)) / (z * 4.)
+        );
+      }
+    }
+
+    static Quaternion fromMatrixN (const Matrix33f &mat) {
+      float tr = mat(0,0) + mat(1,1) + mat(2,2);
+      if (tr > 0) {
+        float w = sqrt (1.f + tr) * 0.5;
+        return Quaternion (
+            (mat(1,2) - mat(2,1)) / (w * 4.),
+            (mat(2,0) - mat(0,2)) / (w * 4.),
+            (mat(0,1) - mat(1,0)) / (w * 4.),
+            w);
+      } else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
+        float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
+        return Quaternion(
+            x,
+            (mat(1,0) + mat(0,1)) / (x * 4.),
+            (mat(2,0) + mat(0,2)) / (x * 4.),
+            (mat(1,2) - mat(2,1)) / (x * 4.)
+        );
+      } else if (mat(1,1) > mat(2,2)) {
+        float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
+        return Quaternion(
+            (mat(1,0) + mat(0,1)) / (y * 4.),
+            y,
+            (mat(2,1) + mat(1,2)) / (y * 4.),
+            (mat(2,0) - mat(0,2)) / (y * 4.)
+        );
+      } else {
+        float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
+        return Quaternion(
+            (mat(2,0) + mat(0,2)) / (z * 4.),
+            (mat(2,1) + mat(1,2)) / (z * 4.),
+            z,
+            (mat(0,1) - mat(1,0)) / (z * 4.)
+        );
+      }
     }
 
     static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {

3rdparty/rbdl/src/Joint.cc

@@ -14,6 +14,21 @@
 #include "rbdl/Model.h"
 #include "rbdl/Joint.h"
 
+#ifdef __APPLE__
+#include <cmath>
+inline void sincos(double x, double * sinp, double * cosp) {
+    return __sincos(x, sinp, cosp);
+}
+#endif
+
+#if _MSC_VER || defined(__QNX__)
+#include <cmath>
+inline void sincos(double x, double * sinp, double * cosp) {
+    *sinp = sin(x);
+    *cosp = cos(x);
+}
+#endif
+
 namespace RigidBodyDynamics {
 
 using namespace Math;

3rdparty/rbdl/tests/BodyTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -14,7 +14,7 @@ const double TEST_PREC = 1.0e-14;
 /* Tests whether the spatial inertia matches the one specified by its
  * parameters
  */
-TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) {
+TEST_CASE (__FILE__"_TestComputeSpatialInertiaFromAbsoluteRadiiGyration", "") {
   Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
 
   Matrix3d inertia_C (
@@ -35,11 +35,11 @@ TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) {
 
   SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia);
 
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_rbi.toMatrix(), TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC);
+  REQUIRE_THAT (inertia_C, AllCloseMatrix(body.mInertia, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyConstructorMassComInertia ) {
+TEST_CASE (__FILE__"_TestBodyConstructorMassComInertia", "") {
   double mass = 1.1;
   Vector3d com (1.5, 1.2, 1.3);
   Matrix3d inertia_C (
@@ -60,11 +60,11 @@ TEST ( TestBodyConstructorMassComInertia ) {
       );
 
   SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia);
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_rbi.toMatrix(), TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC);
+  REQUIRE_THAT (inertia_C, AllCloseMatrix(body.mInertia, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyJoinNullbody ) {
+TEST_CASE (__FILE__"_TestBodyJoinNullbody", "") {
   ClearLogOutput();
   Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
   Body nullbody (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
@@ -75,12 +75,12 @@ TEST ( TestBodyJoinNullbody ) {
   SpatialRigidBodyInertia body_rbi (body.mMass, body.mCenterOfMass, body.mInertia);
   SpatialRigidBodyInertia joined_body_rbi (joined_body.mMass, joined_body.mCenterOfMass, joined_body.mInertia);
 
-  CHECK_EQUAL (1.1, body.mMass);
+  REQUIRE (1.1 == body.mMass);
-  CHECK_ARRAY_CLOSE (body.mCenterOfMass.data(), joined_body.mCenterOfMass.data(), 3, TEST_PREC);
+  REQUIRE_THAT (body.mCenterOfMass, AllCloseMatrix(joined_body.mCenterOfMass, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (body_rbi.toMatrix().data(), joined_body_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (body_rbi.toMatrix(), AllCloseMatrix(joined_body_rbi.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyJoinTwoBodies ) {
+TEST_CASE (__FILE__"_TestBodyJoinTwoBodies", "") {
   ClearLogOutput();
   Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3));
   Body body_b(1.1, Vector3d (1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3));
@@ -99,12 +99,12 @@ TEST ( TestBodyJoinTwoBodies ) {
       2.86, -0, 0, 0, 0, 2.2
       );
 
-  CHECK_EQUAL (2.2, body_joined.mMass);
+  REQUIRE (2.2 == body_joined.mMass);
-  CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3);
+  REQUIRE_THAT (Vector3d (0., 1.3, 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.));
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_joined_rbi.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyJoinTwoBodiesDisplaced ) {
+TEST_CASE (__FILE__"_TestBodyJoinTwoBodiesDisplaced", "") {
   ClearLogOutput();
   Body body_a(1.1, Vector3d (-1.1, 1.3, 0.), Vector3d (3.1, 3.2, 3.3));
   Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3));
@@ -123,14 +123,12 @@ TEST ( TestBodyJoinTwoBodiesDisplaced ) {
       2.86, -0, 0, 0, 0, 2.2
       );
 
-  CHECK_EQUAL (2.2, body_joined.mMass);
+  REQUIRE (2.2 == body_joined.mMass);
-  CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3);
+  REQUIRE_THAT (Vector3d (0., 1.3, 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.));
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_joined_rbi.toMatrix(), TEST_PREC, TEST_PREC));
-
-
 }
 
-TEST ( TestBodyJoinTwoBodiesRotated ) {
+TEST_CASE (__FILE__"_TestBodyJoinTwoBodiesRotated", "") {
   ClearLogOutput();
   Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3));
   Body body_b(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.3, 3.2));
@@ -149,12 +147,12 @@ TEST ( TestBodyJoinTwoBodiesRotated ) {
       0., 0., 0., 0., 0., 2.2
       );
 
-  CHECK_EQUAL (2.2, body_joined.mMass);
+  REQUIRE (2.2 == body_joined.mMass);
-  CHECK_ARRAY_EQUAL (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.));
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_joined_rbi.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyJoinTwoBodiesRotatedAndTranslated ) {
+TEST_CASE (__FILE__"_TestBodyJoinTwoBodiesRotatedAndTranslated", "") {
   ClearLogOutput();
   Body body_a(1.1, Vector3d (0., 0., 0.), Vector3d (3.1, 3.2, 3.3));
   Body body_b(1.1, Vector3d (-1., 1., 0.), Vector3d (3.2, 3.1, 3.3));
@@ -173,12 +171,12 @@ TEST ( TestBodyJoinTwoBodiesRotatedAndTranslated ) {
       0., 0., 0., 0., 0., 2.2
       );
 
-  CHECK_EQUAL (2.2, body_joined.mMass);
+  REQUIRE (2.2 == body_joined.mMass);
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(body_joined.mCenterOfMass, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_joined_rbi.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion ) {
+TEST_CASE (__FILE__"_TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion", "") {
   Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
 
   SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
@@ -191,15 +189,10 @@ TEST ( TestBodyConstructorSpatialRigidBodyInertiaMultiplyMotion ) {
   SpatialVector fv_matrix = rbi.toMatrix() * mv;
   SpatialVector fv_rbi = rbi * mv;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (fv_matrix, AllCloseMatrix(fv_rbi, TEST_PREC, TEST_PREC));
-      fv_matrix.data(),
-      fv_rbi.data(),
-      6,
-      TEST_PREC
-      );
 }
 
-TEST ( TestBodyConstructorSpatialRigidBodyInertia ) {
+TEST_CASE (__FILE__"_TestBodyConstructorSpatialRigidBodyInertia", "") {
   Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
 
   SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
@@ -209,15 +202,10 @@ TEST ( TestBodyConstructorSpatialRigidBodyInertia ) {
       );
   SpatialMatrix spatial_inertia = rbi.toMatrix();
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (spatial_inertia, AllCloseMatrix(rbi.toMatrix(), TEST_PREC, TEST_PREC));
-      spatial_inertia.data(),
-      rbi.toMatrix().data(),
-      36,
-      TEST_PREC
-      );
 }
 
-TEST ( TestBodyConstructorCopySpatialRigidBodyInertia ) {
+TEST_CASE (__FILE__"_TestBodyConstructorCopySpatialRigidBodyInertia", "") {
   Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
 
   SpatialRigidBodyInertia rbi = SpatialRigidBodyInertia(
@@ -235,10 +223,5 @@ TEST ( TestBodyConstructorCopySpatialRigidBodyInertia ) {
   //	cout << "rbi.h = " << rbi.h.transpose() << endl;
   //	cout << "rbi.I = " << endl << rbi.I << endl;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (rbi.toMatrix(), AllCloseMatrix(rbi_from_matrix.toMatrix(), TEST_PREC, TEST_PREC));
-      rbi.toMatrix().data(),
-      rbi_from_matrix.toMatrix().data(),
-      36,
-      TEST_PREC
-      );
 }

3rdparty/rbdl/tests/CMakeLists.txt

@@ -1,14 +1,6 @@
 CMAKE_MINIMUM_REQUIRED (VERSION 3.0)
 
-# Needed for UnitTest++
-LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake )
-
-# Look for unittest++
-FIND_PACKAGE (UnitTest++ REQUIRED)
-INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR})
-
 SET ( TESTS_SRCS
-  main.cc
         MathTests.cc
         SpatialAlgebraTests.cc
         MultiDofTests.cc
@@ -45,7 +37,7 @@ SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
   CXX_EXTENSIONS OFF
   )
 
-ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} )
+ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} main.cc)
 
 SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES
   LINKER_LANGUAGE CXX
@@ -55,13 +47,14 @@ SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES
   CXX_EXTENSIONS OFF
   )
 
+
 SET (RBDL_LIBRARY rbdl)
 IF (RBDL_BUILD_STATIC)
   SET (RBDL_LIBRARY rbdl-static)
 ENDIF (RBDL_BUILD_STATIC)
 
+
 TARGET_LINK_LIBRARIES ( rbdl_tests
-  ${UNITTEST++_LIBRARY}
         ${RBDL_LIBRARY}
         )
 

3rdparty/rbdl/tests/CalcAccelerationsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -15,7 +15,7 @@ using namespace RigidBodyDynamics::Math;
 
 const double TEST_PREC = 1.0e-14;
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointSimple", "") {
   QDDot[0] = 1.;
   ref_body_id = body_a_id;
   point_position = Vector3d (1., 0., 0.);
@@ -23,14 +23,12 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE(0., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (0., 1., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE(1., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE(0., point_acceleration[2], TEST_PREC);
 
   // LOG << "Point accel = " << point_acceleration << endl;
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimpleRotated) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointSimpleRotated", "") {
   Q[0] = 0.5 * M_PI;
 
   ref_body_id = body_a_id;
@@ -40,14 +38,12 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimpleRotated) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC);
 
   // LOG << "Point accel = " << point_acceleration << endl;
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointRotation", "") {
   ref_body_id = 1;
   QDot[0] = 1.;
   point_position = Vector3d (1., 0., 0.);
@@ -55,9 +51,7 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE(-1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC);
 
   ClearLogOutput();
 
@@ -67,12 +61,10 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( 1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC);
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatedBaseSimple) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointRotatedBaseSimple", "") {
   // rotated first joint
 
   ref_body_id = 1;
@@ -81,20 +73,17 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatedBaseSimple) {
   point_position = Vector3d (1., 0., 0.);
   point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
 
-  CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (0., -1., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE(-1., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC);
 
   point_position = Vector3d (-1., 0., 0.);
   point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, ref_body_id, point_position);
 
-  CHECK_CLOSE( 0., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (0., 1., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE( 1., point_acceleration[1], TEST_PREC);
+
-  CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC);
   //	cout << LogOutput.str() << endl;
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatingBodyB) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointRotatingBodyB", "") {
   // rotating second joint, point at third body
 
   ref_body_id = 3;
@@ -104,9 +93,7 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatingBodyB) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE(  0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE(  0., point_acceleration[2], TEST_PREC);
 
   // move it a bit further up (acceleration should stay the same)
   point_position = Vector3d (1., 1., 0.); 
@@ -114,12 +101,10 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotatingBodyB) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); 
-  CHECK_CLOSE(  0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE(  0., point_acceleration[2], TEST_PREC);
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointBodyOrigin) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointBodyOrigin", "") {
   // rotating second joint, point at third body
 
   QDot[0] = 1.;
@@ -130,12 +115,10 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointBodyOrigin) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( -1., point_acceleration[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC )); 
-  CHECK_CLOSE(  0., point_acceleration[1], TEST_PREC);
-  CHECK_CLOSE(  0., point_acceleration[2], TEST_PREC);
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestAccelerationLinearFuncOfQddot) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestAccelerationLinearFuncOfQddot", "") {
   // rotating second joint, point at third body
 
   QDot[0] = 1.1;
@@ -167,30 +150,30 @@ TEST_FIXTURE(FixedBase3DoF, TestAccelerationLinearFuncOfQddot) {
 
   Vector3d acc_new = acc_1 - acc_2;
 
-  CHECK_ARRAY_CLOSE (net_acc.data(), acc_new.data(), 3, TEST_PREC);
+  REQUIRE_THAT(net_acc,AllCloseVector(acc_new, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithUpdateKinematics ) {
+ TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestAccelerationFloatingBaseWithUpdateKinematics", "") {
   ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 
   ClearLogOutput();
   Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), true);
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., -9.81, 0.), accel.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., -9.81, 0.), AllCloseVector(accel, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FloatingBase12DoF, TestAccelerationFloatingBaseWithoutUpdateKinematics ) {
+ TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestAccelerationFloatingBaseWithoutUpdateKinematics", "") {
   ForwardDynamics (*model, Q, QDot, Tau, QDDot);
 
   //ClearLogOutput();
   Vector3d accel = CalcPointAcceleration (*model, Q, QDot, QDDot, child_2_rot_x_id, Vector3d (0., 0., 0.), false);
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), accel.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(accel, TEST_PREC, TEST_PREC));
   //	cout << LogOutput.str() << endl;
   //	cout << accel.transpose() << endl;
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJoint) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointRotationFixedJoint", "") {
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
   unsigned int fixed_body_id = model->AddBody (body_c_id, Xtrans (Vector3d (1., -1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
 
@@ -202,13 +185,10 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJoint) {
   point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position);
   //	cout << LogOutput.str() << endl;
 
-  CHECK_ARRAY_CLOSE (point_acceleration_reference.data(),
+  REQUIRE_THAT (point_acceleration_reference, AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
-      point_acceleration.data(),
-      3, 
-      TEST_PREC);
 }
 
-TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJointRotatedTransform) {
+ TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointRotationFixedJointRotatedTransform", "") {
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
 
   SpatialTransform fixed_transform = Xtrans (Vector3d (1., -1., 0.)) * Xrotz(M_PI * 0.5);
@@ -227,9 +207,6 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotationFixedJointRotatedTransform) {
   point_acceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, fixed_body_id, point_position);
   // cout << LogOutput.str() << endl;
 
-  CHECK_ARRAY_CLOSE (point_acceleration_reference.data(),
+  REQUIRE_THAT (point_acceleration_reference, AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
-      point_acceleration.data(),
-      3, 
-      TEST_PREC);
 }
 

3rdparty/rbdl/tests/CalcVelocitiesTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -58,21 +58,19 @@ struct ModelVelocitiesFixture {
   Vector3d point_position, point_velocity;
 };
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointSimple) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointSimple", "") {
   ref_body_id = 1;
   QDot[0] = 1.;
   point_position = Vector3d (1., 0., 0.);
   point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
 
-  CHECK_CLOSE(0., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE(1., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(0., point_velocity[2], TEST_PREC);
 
   LOG << "Point velocity = " << point_velocity << endl;
   //	cout << LogOutput.str() << endl;
 }
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseSimple) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointRotatedBaseSimple", "") {
   // rotated first joint
 
   ref_body_id = 1;
@@ -81,14 +79,12 @@ TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseSimple) {
   point_position = Vector3d (1., 0., 0.);
   point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
 
-  CHECK_CLOSE(-1., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(-1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE( 0., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_velocity[2], TEST_PREC);
 
   //	cout << LogOutput.str() << endl;
 }
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBodyB) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointRotatingBodyB", "") {
   // rotating second joint, point at third body
 
   ref_body_id = 3;
@@ -98,12 +94,10 @@ TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBodyB) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( 0., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(0., 0., -1.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE( 0., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(-1., point_velocity[2], TEST_PREC);
 }
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBaseXAxis) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointRotatingBaseXAxis", "") {
   // also rotate the first joint and take a point that is
   // on the X direction
 
@@ -115,12 +109,10 @@ TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatingBaseXAxis) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( 0., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(0., 2., -1.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE( 2., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(-1., point_velocity[2], TEST_PREC);
 }
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseXAxis) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointRotatedBaseXAxis", "") {
   // perform the previous test with the first joint rotated by pi/2
   // upwards 
   ClearLogOutput();
@@ -135,12 +127,10 @@ TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointRotatedBaseXAxis) {
 
   //	cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE(-2., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(-2., 0., -1.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE( 0., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(-1., point_velocity[2], TEST_PREC);
 }
 
-TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointBodyOrigin) {
+TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointBodyOrigin", "") {
   // Checks whether the computation is also correct for points at the origin
   // of a body
 
@@ -154,12 +144,10 @@ TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointBodyOrigin) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE( 0., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT(Vector3d(0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE( 1., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE( 0., point_velocity[2], TEST_PREC);
 }
 
-TEST ( FixedJointCalcPointVelocity ) {
+TEST_CASE (__FILE__"_FixedJointCalcPointVelocity", "") {
   // the standard modeling using a null body
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -182,11 +170,11 @@ TEST ( FixedJointCalcPointVelocity ) {
   // cout << LogOutput.str() << endl;
   Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.));
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point0_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(point0_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), point1_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 2., 0.), AllCloseVector(point1_velocity, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointCalcPointVelocityRotated ) {
+TEST_CASE (__FILE__"_FixedJointCalcPointVelocityRotated", "") {
   // the standard modeling using a null body
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -210,6 +198,6 @@ TEST ( FixedJointCalcPointVelocityRotated ) {
   // cout << LogOutput.str() << endl;
   Vector3d point1_velocity = CalcPointVelocity (model, Q, QDot, fixed_body_id, Vector3d (1., 0., 0.));
 
-  CHECK_ARRAY_CLOSE (Vector3d (-1., 0., 0.).data(), point0_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (-1., 0., 0.), AllCloseVector(point0_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (-2., 0., 0.).data(), point1_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (-2., 0., 0.), AllCloseVector(point1_velocity, TEST_PREC, TEST_PREC));
 }

3rdparty/rbdl/tests/CompositeRigidBodyTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -26,7 +26,7 @@ struct CompositeRigidBodyFixture {
   Model *model;
 };
 
-TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsFloatingBase) {
+TEST_CASE_METHOD (CompositeRigidBodyFixture, __FILE__"_TestCompositeRigidBodyForwardDynamicsFloatingBase", "") {
   Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
 
   model->AddBody (0, SpatialTransform(), 
@@ -81,12 +81,12 @@ TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsFlo
 
   InverseDynamics (*model, Q, QDot, QDDot_zero, C);
 
-  CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));
+  REQUIRE (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));
 
-  CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot, AllCloseVector(QDDot_crba, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoF) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestCRBAFloatingBase12DoF", "") {
   MatrixNd H = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count);
 
   VectorNd C = VectorNd::Constant ((size_t) model->dof_count, 0.);
@@ -139,12 +139,12 @@ TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoF) {
   // cout << LogOutput.str() << endl;
   InverseDynamics (*model, Q, QDot, QDDot_zero, C);
 
-  CHECK (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));
+  REQUIRE (LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot_crba));
 
-  CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_crba.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot, AllCloseVector(QDDot_crba, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestCRBAFloatingBase12DoFInverseDynamics", "") {
   MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count);
   MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count);
 
@@ -163,7 +163,7 @@ TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
 
   QDot.setZero();
 
-  assert (model->dof_count == 12);
+  REQUIRE (model->dof_count == 12);
 
   UpdateKinematicsCustom (*model, &Q, NULL, NULL);
   CompositeRigidBodyAlgorithm (*model, Q, H_crba, false);
@@ -194,10 +194,10 @@ TEST_FIXTURE(FloatingBase12DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
   //	cout << "H (crba) = " << endl << H_crba << endl;
   //	cout << "H (id) = " << endl << H_id << endl;
 
-  CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC);
+  REQUIRE_THAT (H_crba, AllCloseMatrix(H_id, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(FixedBase6DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_TestCRBAFloatingBase6DoFInverseDynamics", "") {
   MatrixNd H_crba = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count);
   MatrixNd H_id = MatrixNd::Zero ((size_t) model->dof_count, (size_t) model->dof_count);
 
@@ -210,7 +210,7 @@ TEST_FIXTURE(FixedBase6DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
 
   QDot.setZero();
 
-  assert (model->dof_count == 6);
+  REQUIRE (model->dof_count == 6);
 
   UpdateKinematicsCustom (*model, &Q, NULL, NULL);
   CompositeRigidBodyAlgorithm (*model, Q, H_crba, false);
@@ -239,10 +239,10 @@ TEST_FIXTURE(FixedBase6DoF, TestCRBAFloatingBase12DoFInverseDynamics) {
     H_id.block<6, 1>(0, i) = H_col;
   }
 
-  CHECK_ARRAY_CLOSE (H_crba.data(), H_id.data(), model->dof_count * model->dof_count, TEST_PREC);
+  REQUIRE_THAT (H_crba, AllCloseMatrix(H_id, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsSpherical) {
+TEST_CASE_METHOD (CompositeRigidBodyFixture, __FILE__"_TestCompositeRigidBodyForwardDynamicsSpherical", "") {
   Body base_body(1., Vector3d (0., 0., 0.), Vector3d (1., 2., 3.));
 
   model->AddBody(0, SpatialTransform(), Joint(JointTypeSpherical), base_body);
@@ -257,5 +257,5 @@ TEST_FIXTURE(CompositeRigidBodyFixture, TestCompositeRigidBodyForwardDynamicsSph
       0., 0., 3.
       );
 
-  CHECK_ARRAY_CLOSE (H_ref.data(), H.data(), 9, TEST_PREC);
+  REQUIRE_THAT (H_ref, AllCloseMatrix(H, TEST_PREC, TEST_PREC));
 }

3rdparty/rbdl/tests/ContactsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -103,7 +103,7 @@ struct FixedBase6DoF9DoF {
 // 
 // ForwardDynamicsConstraintsDirect 
 // 
-TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
+TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectSimple", "") {
   Model model;
   model.gravity = Vector3d  (0., -9.81, 0.);
   Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
@@ -152,19 +152,14 @@ TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
 
   Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
-      Vector3d (0., 0., 0.).data(),
-      point_acceleration.data(),
-      3,
-      TEST_PREC
-      );
 
   // cout << "LagrangianSimple Logoutput Start" << endl;
   // cout << LogOutput.str() << endl;
   // cout << "LagrangianSimple Logoutput End" << endl;
 }
 
-TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
+TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectMoving", "") {
   Model model;
   model.gravity = Vector3d  (0., -9.81, 0.);
   Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
@@ -215,12 +210,7 @@ TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
 
   Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
-      Vector3d (0., 0., 0.).data(),
-      point_acceleration.data(),
-      3,
-      TEST_PREC
-      );
 
   // cout << "LagrangianSimple Logoutput Start" << endl;
   // cout << LogOutput.str() << endl;
@@ -230,7 +220,7 @@ TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
 //
 // ForwardDynamicsContacts
 // 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContact", "") {
   contact_normal.set (0., 1., 0.);
   constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal);
   ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
@@ -255,13 +245,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
   point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
   point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 
-  CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
+  REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
+  REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts), TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContactRotated", "") {
   Q[0] = 0.6;
   Q[3] =   M_PI * 0.6;
   Q[4] = 0.1;
@@ -289,10 +279,10 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
   point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
   point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
 
-  CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
+  REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC);
+  REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts_opt), TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts_opt, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts_opt, TEST_PREC, TEST_PREC));
 }
 
 //
@@ -302,7 +292,7 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
 // for the example model in FixedBase6DoF and a moving state (i.e. a
 // nonzero QDot)
 // 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContactRotatedMoving", "") {
   Q[0] = 0.6;
   Q[3] =   M_PI * 0.6;
   Q[4] = 0.1;
@@ -335,14 +325,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving)
   point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 
   // check whether FDContactsLagrangian and FDContactsOld match
-  CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
+  REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
-
+  REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts), TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
+  REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
 }
 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptDoubleContact", "") {
   ConstraintSet constraint_set_lagrangian;
 
   constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal);
@@ -368,20 +357,16 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) {
   point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 
   // check whether FDContactsLagrangian and FDContacts match
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
 
   // check whether the point accelerations match
-  CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
 
   // check whether the generalized accelerations match
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptDoubleContactRepeated", "") {
   // makes sure that all variables in the constraint set gets reset
   // properly when making repeated calls to ForwardDynamicsContacts.
   ConstraintSet constraint_set_lagrangian;
@@ -412,21 +397,18 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) {
   point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
   point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
 
+
   // check whether FDContactsLagrangian and FDContacts match
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
 
   // check whether the point accelerations match
-  CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
 
   // check whether the generalized accelerations match
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) {
+TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContact", "") {
   ConstraintSet constraint_set_lagrangian;
 
   constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
@@ -464,19 +446,15 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) {
 
 //  cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
 
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving) {
+TEST_CASE_METHOD (FixedBase6DoF9DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving", "") {
   ConstraintSet constraint_set_lagrangian;
 
   constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
@@ -517,27 +495,23 @@ TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMulti
   ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
 //  cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
   point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
   point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate) {
+TEST_CASE_METHOD (FixedBase6DoF9DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate", "") {
   ConstraintSet constraint_set_lagrangian;
 
   constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
@@ -584,27 +558,23 @@ TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMulti
   ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
 //  cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
   point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
   point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase) {
+TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_ForwardDynamicsContactsMultipleContactsFloatingBase", "") {
   ConstraintSet constraint_set_lagrangian;
 
   constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
@@ -653,27 +623,23 @@ TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleCon
 //  cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
 //  cout << LogOutput.str() << endl;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
-      constraint_set_lagrangian.force.data(),
-      constraint_set.force.data(),
-      constraint_set.size(), TEST_PREC
-      );
   
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
   point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
   point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
 
-  CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
+  REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
 
-  CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) {
+TEST_CASE_METHOD (Human36, __FILE__"_ForwardDynamicsContactsFixedBody", "") {
   VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size()));
   VectorNd qddot_sparse (VectorNd::Zero(qddot.size()));
 
@@ -687,11 +653,11 @@ TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) {
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_sparse);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot);
 
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.);
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot, TEST_PREC * qddot_lagrangian.norm() * 10., TEST_PREC * qddot_lagrangian.norm() * 10.));
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.);
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm() * 10., TEST_PREC * qddot_lagrangian.norm() * 10.));
 }
 
-TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) {
+TEST_CASE_METHOD (Human36, __FILE__"_ForwardDynamicsContactsImpulses", "") {
   VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size()));
 
   for (unsigned int i = 0; i < q.size(); i++) {
@@ -719,6 +685,6 @@ TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) {
   Vector3d heel_left_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootLeft], heel_point);
   Vector3d heel_right_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootRight], heel_point);
 
-  CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_left_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_left_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_right_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_right_velocity, TEST_PREC, TEST_PREC));
 }

3rdparty/rbdl/tests/CustomConstraintsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 #include "rbdl/rbdl.h"
 #include "rbdl/Constraints.h"
 #include <cassert>
@@ -335,8 +335,7 @@ struct DoublePerpendicularPendulumCustomConstraint {
 
 
 
-TEST(CustomConstraintCorrectnessTest) {
+TEST_CASE (__FILE__"_CustomConstraintCorrectnessTest", "") {
-
   //Test to add:
   //  Jacobian vs. num Jacobian
   DoublePerpendicularPendulumCustomConstraint dbcc
@@ -430,8 +429,8 @@ TEST(CustomConstraintCorrectnessTest) {
   CalcConstraintsVelocityError(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,errd,true);
 
 
-  CHECK(err.norm()  >= qError);
+  REQUIRE (err.norm() >= qError);
-  CHECK(errd.norm() >= qDotError);
+  REQUIRE (errd.norm() >= qDotError);
 
   //Solve for the initial q and qdot terms that satisfy the constraints
   VectorNd qAsm,qDotAsm,w;
@@ -460,14 +459,9 @@ TEST(CustomConstraintCorrectnessTest) {
 
   //The constraint errors at the position and velocity level
   //must be zero before the accelerations can be tested.
-  for(unsigned int i=0; i<err.rows();++i){
+  VectorNd target(dbcc.cs.size());
-    CHECK_CLOSE(0,err[i],TEST_PREC);
+  REQUIRE_THAT(target, AllCloseVector(err, TEST_PREC, TEST_PREC));
-  }
+  REQUIRE_THAT(target, AllCloseVector(errd, TEST_PREC, TEST_PREC));
-  for(unsigned int i=0; i<errd.rows();++i){
-    CHECK_CLOSE(0,errd[i],TEST_PREC);
-  }
-
-
 
   //Evaluate the accelerations of the constrained pendulum and
   //compare those to the joint-coordinate pendulum
@@ -483,24 +477,16 @@ TEST(CustomConstraintCorrectnessTest) {
   ForwardDynamicsConstraintsDirect(dba.model, dba.q,  dba.qd,
                                    dba.tau,   dba.cs, dba.qdd);
 
-  for(unsigned int i = 0; i < dba.cs.G.rows(); ++i){
+  REQUIRE_THAT (dba.cs.G, AllCloseMatrix(dbcc.cs.G, TEST_PREC, TEST_PREC));
-    for(unsigned int j=0; j< dba.cs.G.cols();++j){
+  REQUIRE_THAT (dba.cs.gamma, AllCloseVector(dbcc.cs.gamma, TEST_PREC, TEST_PREC));
-      CHECK_CLOSE(dba.cs.G(i,j),dbcc.cs.G(i,j),TEST_PREC);
-    }
-  }
-
-  for(unsigned int i = 0; i < dba.cs.gamma.rows(); ++i){
-    CHECK_CLOSE(dba.cs.gamma[i],dbcc.cs.gamma[i],TEST_PREC);
-  }
 
+  //REQUIRE_THAT (dba.cs.constraintAxis, AllCloseVector(dbcc.cs.constraintAxis, TEST_PREC, TEST_PREC)); //does not work
   for(unsigned int i=0; i < dba.cs.constraintAxis.size(); ++i){
     for(unsigned int j=0; j< dba.cs.constraintAxis[0].rows(); ++j){
-      CHECK_CLOSE(dba.cs.constraintAxis[i][j],
+      REQUIRE_THAT (dba.cs.constraintAxis[i][j], IsClose(dbcc.cs.constraintAxis[i][j], TEST_PREC, TEST_PREC));
-                  dbcc.cs.constraintAxis[i][j],TEST_PREC);
     }
   }
 
-
   SpatialVector a010c =
       CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
                           dbcc.idB1,Vector3d(0.,0.,0.),true);
@@ -511,10 +497,7 @@ TEST(CustomConstraintCorrectnessTest) {
       CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
                           dbcc.idB2,Vector3d(dbcc.l2,0.,0.),true);
 
-  for(unsigned int i=0; i<6;++i){
+  REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
+  REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
+  REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
-  }
-
 }

3rdparty/rbdl/tests/CustomJointMultiBodyTests.cc

@@ -4,7 +4,7 @@
  */
 
 
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -504,8 +504,7 @@ struct CustomJointMultiBodyFixture {
 //
 //==============================================================================
 
-TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) {
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_UpdateKinematics", "") {
-
   VectorNd test;
 
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
@@ -535,40 +534,25 @@ TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) {
     //                   ].E;
     // Matrix3d Eerr = Eref-Ecus;
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E,
-      reference_model.at(idx).X_base[
+      AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC)
-        reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-        ].E.data(),
-      custom_model.at(idx).X_base[
-        custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
-        ].E.data(),
-      9,
-      TEST_PREC);
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E,
-      reference_model.at(idx).v[
+      AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC)
-        reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-        ].data(),
-      custom_model.at(idx).v[
-        custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
-        ].data(),
-      6,
-      TEST_PREC);
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)],
-      reference_model.at(idx).a[
+      AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC, TEST_PREC)
-        reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-        ].data(),
+
-      custom_model.at(idx).a[
+    REQUIRE_THAT (reference_model.at(idx).a[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)],
-        custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+      AllCloseVector(custom_model.at(idx).a[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC, TEST_PREC)
-        ].data(),
+    );
-      6,
-      TEST_PREC);
   }
 }
 
-TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) {
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_UpdateKinematicsCustom", "") {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -583,15 +567,9 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) {
                             &q.at(idx), NULL, NULL);
 
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E,
-      reference_model.at(idx).X_base[
+      AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC)
-        reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-        ].E.data(),
-      custom_model.at(idx).X_base[
-        custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
-        ].E.data(),
-      9,
-      TEST_PREC);
 
 
     //velocity
@@ -602,15 +580,9 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) {
                             &q.at(idx),
                             &qdot.at(idx), NULL);
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)],
-          reference_model.at(idx).v[
+      AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC,TEST_PREC)
-            reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-            ].data(),
-          custom_model.at(idx).v[
-            custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
-            ].data(),
-          6,
-          TEST_PREC);
 
 
     //All
@@ -624,21 +596,13 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) {
                             &qdot.at(idx),
                             &qddot.at(idx));
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).a[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)],
-      reference_model.at(idx).a[
+      AllCloseVector(custom_model.at(idx).a[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC, TEST_PREC)
-        reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)
+    );
-        ].data(),
+  }   
-      custom_model.at(idx).a[
-        custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)
-        ].data(),
-      6,
-      TEST_PREC);
 }
 
-
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_Jacobians", "") {
-}
-
-TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) {
 
   for(int idx = 0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
@@ -676,14 +640,7 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) {
                               custom_body_id.at(idx).at(NUMBER_OF_BODIES-1),
                               Gcus);
 
-    for(int i=0; i<6;++i){
+    REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          Gref(i,j),
-          Gcus(i,j),
-          TEST_PREC);
-      }
-    }
 
     //Check the 6d point Jacobian
     Vector3d point_position (1.1, 1.2, 2.1);
@@ -701,14 +658,7 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) {
                          point_position,
                          Gcus);
 
-    for(int i=0; i<6;++i){
+    REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          Gref(i,j),
-          Gcus(i,j),
-          TEST_PREC);
-      }
-    }
 
     //Check the 3d point Jacobian
     MatrixNd GrefPt = MatrixNd::Constant (3,
@@ -730,20 +680,11 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) {
                        point_position,
                        GcusPt);
 
-    for(int i=0; i<3;++i){
+    REQUIRE_THAT (GrefPt, AllCloseMatrix(GcusPt, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          GrefPt(i,j),
-          GcusPt(i,j),
-          TEST_PREC);
-      }
   }
 }
 
-}
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_InverseDynamics", "") {
-
-TEST_FIXTURE (CustomJointMultiBodyFixture, InverseDynamics) {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -770,17 +711,11 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, InverseDynamics) {
 
     VectorNd tauErr = tauRef-tauCus;
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (tauRef, AllCloseVector(tauCus, TEST_PREC, TEST_PREC));
-      tauRef.data(),
+  }
-      tauCus.data(),
-      tauRef.rows(),
-      TEST_PREC);
 }
 
-}
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_CompositeRigidBodyAlgorithm", "") {
-
-TEST_FIXTURE (CustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm) {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -844,15 +779,11 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm) {
                             c_cus * -1. + tau.at(idx),
                             qddot_crba_cus);
 
-    CHECK_ARRAY_CLOSE(qddot_crba_ref.data(),
+    REQUIRE_THAT(qddot_crba_ref, AllCloseVector(qddot_crba_cus, TEST_PREC, TEST_PREC));
-                      qddot_crba_cus.data(),
-                      dof,
-                      TEST_PREC);
   }
 }
 
-TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamics) {
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_ForwardDynamics", "") {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -878,23 +809,16 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamics) {
                     tau.at(idx),
                     qddotCus);
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (qddotRef, AllCloseVector(qddotCus, TEST_PREC, TEST_PREC));
-      qddotRef.data(),
+  }
-      qddotCus.data(),
-      dof,
-      TEST_PREC);
 }
 
-}
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_CalcMInvTimestau", "") {
-
-TEST_FIXTURE (CustomJointMultiBodyFixture, CalcMInvTimestau) {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
       q.at(idx)[i]    = (i+0.1) * 9.133758561390194e-01;
       tau.at(idx)[i]  = (i+0.1) * 9.754040499940952e-02;
-
     }
 
     //reference
@@ -915,16 +839,11 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, CalcMInvTimestau) {
                      true);
 
     //check.
-    CHECK_ARRAY_CLOSE(qddot_minv_ref.data(),
+    REQUIRE_THAT(qddot_minv_ref, AllCloseVector(qddot_minv_cus, TEST_PREC, TEST_PREC));
-                      qddot_minv_cus.data(),
+  }
-                      dof,
-                      TEST_PREC);
 }
 
-}
+TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_ForwardDynamicsContactsKokkevis", ""){
-
-TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamicsContactsKokkevis){
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     //Adding a 1 constraint to a system with 1 dof is
@@ -1008,18 +927,10 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamicsContactsKokkevis){
       VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
       VectorNd qddot_error = qddot_ref - qddot_cus;
 
-      CHECK_ARRAY_CLOSE (qdot_plus_ref.data(),
+      REQUIRE_THAT (qdot_plus_ref, AllCloseVector(qdot_plus_cus, TEST_PREC, TEST_PREC));
-                         qdot_plus_cus.data(),
+      REQUIRE_THAT (qddot_ref, AllCloseVector(qddot_cus, TEST_PREC, TEST_PREC));
-                         dof,
-                         TEST_PREC);
-
-      CHECK_ARRAY_CLOSE (qddot_ref.data(),
-                         qddot_cus.data(),
-                         dof,
-                         TEST_PREC);
     }
   }
-
 }
 
 //

3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc

@@ -4,7 +4,7 @@
  */
 
 
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -370,7 +370,7 @@ struct CustomJointSingleBodyFixture {
 //
 //==============================================================================
 
-TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) {
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_UpdateKinematics", "") {
 
   VectorNd test;
 
@@ -392,27 +392,14 @@ TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) {
                       qdot.at(idx),
                       qddot.at(idx));
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx)].E, AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx)].E, TEST_PREC, TEST_PREC));
-      reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(),
+    REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx)], AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx)], TEST_PREC, TEST_PREC));
-        custom_model.at(idx).X_base[    custom_body_id.at(idx)].E.data(),
+    REQUIRE_THAT (reference_model.at(idx).a[reference_body_id.at(idx)], AllCloseVector(custom_model.at(idx).a[custom_body_id.at(idx)], TEST_PREC, TEST_PREC));
-      9,
-      TEST_PREC);
-
-    CHECK_ARRAY_CLOSE (
-      reference_model.at(idx).v[reference_body_id.at(idx)].data(),
-         custom_model.at(idx).v[   custom_body_id.at(idx)].data(),
-      6,
-      TEST_PREC);
-
-    CHECK_ARRAY_CLOSE (
-      reference_model.at(idx).a[reference_body_id.at(idx)].data(),
-         custom_model.at(idx).a[   custom_body_id.at(idx)].data(),
-      6,
-      TEST_PREC);
   }
 }
 
-TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) {
+
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_UpdateKinematicsCustom", "") {
 
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
@@ -427,13 +414,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) {
     UpdateKinematicsCustom (custom_model.at(idx),
                             &q.at(idx), NULL, NULL);
 
-
+    REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx)].E, AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx)].E, TEST_PREC, TEST_PREC));
-    CHECK_ARRAY_CLOSE (
-      reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(),
-         custom_model.at(idx).X_base[   custom_body_id.at(idx)].E.data(),
-      9,
-      TEST_PREC);
-
 
     //velocity
     UpdateKinematicsCustom (reference_model.at(idx),
@@ -445,12 +426,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) {
                             &qdot.at(idx),
                             NULL);
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx)], AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx)], TEST_PREC, TEST_PREC));
-          reference_model.at(idx).v[reference_body_id.at(idx)].data(),
-             custom_model.at(idx).v[   custom_body_id.at(idx)].data(),
-          6,
-          TEST_PREC);
-
 
     //All
     UpdateKinematicsCustom (reference_model.at(idx),
@@ -463,18 +439,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) {
                             &qdot.at(idx),
                             &qddot.at(idx));
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (reference_model.at(idx).a[reference_body_id.at(idx)], AllCloseVector(custom_model.at(idx).a[custom_body_id.at(idx)], TEST_PREC, TEST_PREC));
-      reference_model.at(idx).a[reference_body_id.at(idx)].data(),
+  }  
-         custom_model.at(idx).a[   custom_body_id.at(idx)].data(),
-      6,
-      TEST_PREC);
 }
 
-
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_Jacobians", "") {
-}
-
-TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -511,14 +480,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
                               custom_body_id.at(idx),
                               Gcus);
 
-    for(int i=0; i<6;++i){
+    REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          Gref(i,j),
-          Gcus(i,j),
-          TEST_PREC);
-      }
-    }
     
     //Check the 6d point Jacobian
     Vector3d point_position (1.1, 1.2, 2.1);
@@ -535,16 +497,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
                          point_position,
                          Gcus);
 
-    for(int i=0; i<6;++i){
+    REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          Gref(i,j),
-          Gcus(i,j),
-          TEST_PREC);
-      }
-    }
-
-
 
     //Check the 3d point Jacobian
     MatrixNd GrefPt = MatrixNd::Constant (3,
@@ -554,8 +507,6 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
                                           reference_model.at(idx).dof_count,
                                           0.);
 
-
-
     CalcPointJacobian (reference_model.at(idx),
                        q.at(idx),
                        reference_body_id.at(idx),
@@ -568,20 +519,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
                        point_position,
                        GcusPt);
 
-    for(int i=0; i<3;++i){
+    REQUIRE_THAT (GrefPt, AllCloseMatrix(GcusPt, TEST_PREC, TEST_PREC));
-      for(unsigned int j = 0; j < dof; ++j){
-        CHECK_CLOSE (
-          GrefPt(i,j),
-          GcusPt(i,j),
-          TEST_PREC);
-      }
   }
 }
 
-}
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_InverseDynamics", "") {
-
-TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -609,17 +551,12 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) {
 
     VectorNd tauErr = tauRef-tauCus;
 
-    CHECK_ARRAY_CLOSE (
+    REQUIRE_THAT (tauRef, AllCloseVector(tauCus, TEST_PREC, TEST_PREC));
-      tauRef.data(),
-      tauCus.data(),
-      tauRef.rows(),
-      TEST_PREC);
   }
 
 }
 
-TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) {
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_CompositeRigidBodyAlgorithm", "") {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -683,15 +620,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) {
                             c_cus * -1. + tau.at(idx),
                             qddot_crba_cus);
 
-    CHECK_ARRAY_CLOSE(qddot_crba_ref.data(),
+    REQUIRE_THAT(qddot_crba_ref, AllCloseVector(qddot_crba_cus, TEST_PREC, TEST_PREC));
-                      qddot_crba_cus.data(),
-                      dof,
-                      TEST_PREC);
   }
 }
 
-TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) {
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_ForwardDynamics", "") {
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     for (unsigned int i = 0; i < dof; i++) {
@@ -717,15 +650,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) {
                     tau.at(idx),
                     qddotCus);
 
-    CHECK_ARRAY_CLOSE ( qddotRef.data(),
+    REQUIRE_THAT ( qddotRef, AllCloseVector(qddotCus, TEST_PREC, TEST_PREC));
-                        qddotCus.data(),
+  }
-                        dof,
-                        TEST_PREC);
 }
 
-}
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_CalcMInvTimestau", "") {
-
-TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) {
 
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
@@ -753,16 +682,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) {
                      qddot_minv_cus,
                      true);
     //check.
-    CHECK_ARRAY_CLOSE(qddot_minv_ref.data(),
+    REQUIRE_THAT(qddot_minv_ref, AllCloseVector(qddot_minv_cus, TEST_PREC, TEST_PREC));
-                      qddot_minv_cus.data(),
+  }
-                      dof,
-                      TEST_PREC);
 }
 
-}
+TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_ForwardDynamicsContactsKokkevis", ""){
-
-TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){
-
   for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
     unsigned int dof = reference_model.at(idx).dof_count;
     //Adding a 1 constraint to a system with 1 dof is
@@ -842,18 +766,8 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){
       VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
       VectorNd qddot_error     = qddot_ref     - qddot_cus;
 
-      CHECK_ARRAY_CLOSE (qdot_plus_ref.data(),
+      REQUIRE_THAT (qdot_plus_ref, AllCloseVector(qdot_plus_cus, TEST_PREC, TEST_PREC));
-                         qdot_plus_cus.data(),
+      REQUIRE_THAT (qddot_ref, AllCloseVector(qddot_cus, TEST_PREC, TEST_PREC));
-                         dof,
-                         TEST_PREC);
-
-      CHECK_ARRAY_CLOSE (qddot_ref.data(),
-                         qddot_cus.data(),
-                         dof,
-                         TEST_PREC);
     }
   }
-
 }
-
-

3rdparty/rbdl/tests/CustomJointTests.cc

@@ -73,7 +73,7 @@ struct CustomEulerZYXJoint : public CustomJoint {
     const Math::VectorNd &q
     ) {
     // TODO
-    assert (false && "Not yet implemented!");
+    REQUIRE (false && "Not yet implemented!");
   }
 };
 

3rdparty/rbdl/tests/DynamicsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 #include <limits>
@@ -30,7 +30,7 @@ struct DynamicsFixture {
   Model *model;
 };
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicSingleChain", "") {
   Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
   Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -53,10 +53,10 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) {
     LOG << "a[" << i << "]     = " << model->a[i] << endl;
   }
 
-  CHECK_EQUAL (-4.905, QDDot[0]);
+  REQUIRE (-4.905 == QDDot[0]);
 }
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicSpatialInertiaSingleChain", "") {
   // This function checks the value for a non-trivial spatial inertia
   Body body(1., Vector3d (1.5, 1., 1.), Vector3d (1., 2., 3.));
 
@@ -82,10 +82,10 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) {
     LOG << "a[" << i << "]     = " << model->a[i] << endl;
   }
 
-  CHECK_EQUAL (-2.3544, QDDot[0]);
+  REQUIRE (-2.3544 == QDDot[0]);
 }
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicDoubleChain", "") {
   Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
   Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -117,11 +117,11 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) {
 
   //  cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE (-5.88600000000000E+00, QDDot[0], TEST_PREC);
+  REQUIRE_THAT (-5.88600000000000E+00, IsClose(QDDot[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE ( 3.92400000000000E+00, QDDot[1], TEST_PREC);
+  REQUIRE_THAT ( 3.92400000000000E+00, IsClose(QDDot[1], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicTripleChain) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicTripleChain", "") {
   Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
   Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -158,12 +158,10 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicTripleChain) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE (-6.03692307692308E+00, QDDot[0], TEST_PREC);
+  REQUIRE_THAT (Vector3d(-6.03692307692308E+00, 3.77307692307692E+00, 1.50923076923077E+00), AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE ( 3.77307692307692E+00, QDDot[1], TEST_PREC);
-  CHECK_CLOSE ( 1.50923076923077E+00, QDDot[2], TEST_PREC);
 }
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain3D) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicDoubleChain3D", "") {
   Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
   Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -195,11 +193,11 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain3D) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE (-3.92400000000000E+00, QDDot[0], TEST_PREC);
+  REQUIRE_THAT (-3.92400000000000E+00, IsClose(QDDot[0], TEST_PREC, TEST_PREC));
-  CHECK_CLOSE ( 0.00000000000000E+00, QDDot[1], TEST_PREC);
+  REQUIRE_THAT ( 0.00000000000000E+00, IsClose(QDDot[1], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSimpleTree3D) {
+TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicSimpleTree3D", "") {
   Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
   Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -246,14 +244,16 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSimpleTree3D) {
 
   // cout << LogOutput.str() << endl;
 
-  CHECK_CLOSE (-1.60319066147860E+00, QDDot[0], TEST_PREC);
+  VectorNd target = VectorNd::Zero (5);
-  CHECK_CLOSE (-5.34396887159533E-01, QDDot[1], TEST_PREC);
+  target[0] = -1.60319066147860E+00;
-  CHECK_CLOSE ( 4.10340466926070E+00, QDDot[2], TEST_PREC);
+  target[1] = -5.34396887159533E-01;
-  CHECK_CLOSE ( 2.67198443579767E-01, QDDot[3], TEST_PREC);
+  target[2] = 4.10340466926070E+00;
-  CHECK_CLOSE ( 5.30579766536965E+00, QDDot[4], TEST_PREC);
+  target[3] = 2.67198443579767E-01;
+  target[4] = 5.30579766536965E+00;
+  REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 }
 
-TEST (TestForwardDynamicsLagrangian) {
+TEST_CASE (__FILE__"_TestForwardDynamicsLagrangian", "") {
   Model model;
   Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
 
@@ -300,8 +300,8 @@ TEST (TestForwardDynamicsLagrangian) {
   ForwardDynamics(model, Q, QDot, Tau, QDDot_aba);
   ForwardDynamicsLagrangian(model, Q, QDot, Tau, QDDot_lagrangian);
 
-  CHECK_EQUAL (QDDot_aba.size(), QDDot_lagrangian.size());
+  REQUIRE (QDDot_aba.size() == QDDot_lagrangian.size());
-  CHECK_ARRAY_CLOSE (QDDot_aba.data(), QDDot_lagrangian.data(), QDDot_aba.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_aba, AllCloseVector(QDDot_lagrangian, TEST_PREC, TEST_PREC));
 }
 
 /*
@@ -311,7 +311,7 @@ TEST (TestForwardDynamicsLagrangian) {
  * due to the missing gravity term. But as the test now works, I just leave
  * it here.
  */
-TEST (TestForwardDynamics3DoFModel) {
+TEST_CASE (__FILE__"_TestForwardDynamics3DoFModel", "") {
   Model model;
 
   model.gravity = Vector3d (0., -9.81, 0.);
@@ -348,7 +348,7 @@ TEST (TestForwardDynamics3DoFModel) {
 
   QDDot_ref[0] = 3.301932144386186;
 
-  CHECK_ARRAY_CLOSE (QDDot_ref.data(), QDDot.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_ref, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 }
 
 /*
@@ -359,7 +359,7 @@ TEST (TestForwardDynamics3DoFModel) {
  * Running the CRBA after the InverseDynamics calculation fixes this. This
  * test ensures that the error does not happen when calling ForwardLagrangian.
  */
-TEST (TestForwardDynamics3DoFModelLagrangian) {
+TEST_CASE (__FILE__"_TestForwardDynamics3DoFModelLagrangian", "") {
   Model model;
 
   model.gravity = Vector3d (0., -9.81, 0.);
@@ -402,14 +402,14 @@ TEST (TestForwardDynamics3DoFModelLagrangian) {
 //  cout << QDDot_lagrangian << endl;
 //  cout << LogOutput.str() << endl;
 
-  CHECK_ARRAY_CLOSE (QDDot_ab.data(), QDDot_lagrangian.data(), QDDot_ab.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot_ab, AllCloseVector(QDDot_lagrangian, TEST_PREC, TEST_PREC));
 }
 
 /*
  * This is a test for a model where I detected incosistencies between the
  * Lagragian method and the ABA.
  */
-TEST (TestForwardDynamicsTwoLegModelLagrangian) {
+TEST_CASE (__FILE__"_TestForwardDynamicsTwoLegModelLagrangian", "") {
   Model *model = NULL;
 
   unsigned int hip_id,
@@ -574,12 +574,12 @@ TEST (TestForwardDynamicsTwoLegModelLagrangian) {
   ClearLogOutput();
   ForwardDynamicsLagrangian (*model, Q, QDot, Tau, QDDot);
 
-  CHECK_ARRAY_CLOSE (QDDotABA.data(), QDDot.data(), QDDotABA.size(), TEST_PREC);
+  REQUIRE_THAT (QDDotABA, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
 
   delete model;
 }
 
-TEST_FIXTURE(FixedAndMovableJoint, TestForwardDynamicsFixedJoint) {
+TEST_CASE_METHOD (FixedAndMovableJoint, __FILE__"_TestForwardDynamicsFixedJoint", "") {
   Q_fixed[0] = 1.1;
   Q_fixed[1] = 2.2;
 
@@ -603,14 +603,14 @@ TEST_FIXTURE(FixedAndMovableJoint, TestForwardDynamicsFixedJoint) {
   C_fixed[1] = C_movable[2];
 
   VectorNd QDDot_fixed_emulate(2);
-  CHECK (LinSolveGaussElimPivot (H_fixed, C_fixed * -1. + Tau_fixed, QDDot_fixed_emulate));
+  REQUIRE (LinSolveGaussElimPivot (H_fixed, C_fixed * -1. + Tau_fixed, QDDot_fixed_emulate));
 
   ForwardDynamics (*model_fixed, Q_fixed, QDot_fixed, Tau_fixed, QDDot_fixed);
 
-  CHECK_ARRAY_CLOSE (QDDot_fixed_emulate.data(), QDDot_fixed.data(), 2, TEST_PREC);
+  REQUIRE_THAT (QDDot_fixed_emulate, AllCloseVector(QDDot_fixed, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(FixedAndMovableJoint, TestInverseDynamicsFixedJoint) {
+TEST_CASE_METHOD (FixedAndMovableJoint, __FILE__"_TestInverseDynamicsFixedJoint", "") {
   Q_fixed[0] = 1.1;
   Q_fixed[1] = 2.2;
 
@@ -631,10 +631,10 @@ TEST_FIXTURE(FixedAndMovableJoint, TestInverseDynamicsFixedJoint) {
   Tau_2dof[0] = Tau[0];
   Tau_2dof[1] = Tau[2];
 
-  CHECK_ARRAY_CLOSE (Tau_2dof.data(), Tau_fixed.data(), 2, TEST_PREC);
+  REQUIRE_THAT (Tau_2dof, AllCloseVector(Tau_fixed, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( FloatingBase12DoF, TestForwardDynamicsLagrangianPrealloc ) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestForwardDynamicsLagrangianPrealloc", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     Q[i] = static_cast<double>(i + 1) * 0.1;
     QDot[i] = static_cast<double>(i + 1) * 1.1;
@@ -666,10 +666,10 @@ TEST_FIXTURE ( FloatingBase12DoF, TestForwardDynamicsLagrangianPrealloc ) {
       &C
       );
 
-  CHECK_ARRAY_EQUAL (QDDot.data(), QDDot_prealloc.data(), model->dof_count);
+  REQUIRE_THAT (QDDot, AllCloseVector(QDDot_prealloc));
 }
 
-TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) {
+TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesTau", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     Q[i] = rand() / static_cast<double>(RAND_MAX);
     Tau[i] = rand() / static_cast<double>(RAND_MAX);
@@ -683,10 +683,10 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) {
   VectorNd qddot_minv (Q);
   CalcMInvTimesTau (*model, Q, Tau, qddot_minv);
 
-  CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
+  REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) {
+TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesTauReuse", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     Q[i] = rand() / static_cast<double>(RAND_MAX);
     Tau[i] = rand() / static_cast<double>(RAND_MAX);
@@ -710,11 +710,11 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) {
 
     CalcMInvTimesTau (*model, Q, Tau, qddot_minv, false);
 
-    CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
+    REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
   }
 }
 
-TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesNonZeroQDotKinematicsUpdate) {
+TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesNonZeroQDotKinematicsUpdate", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     Q[i] = rand() / static_cast<double>(RAND_MAX);
     QDot[i] = rand() / static_cast<double>(RAND_MAX);
@@ -731,5 +731,5 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesNonZeroQDotKinematicsUpdate) {
   VectorNd qddot_minv (Q);
   CalcMInvTimesTau (*model, Q, Tau, qddot_minv);
 
-  CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
+  REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
 }

3rdparty/rbdl/tests/FloatingBaseTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -34,7 +34,7 @@ struct FloatingBaseFixture {
   VectorNd q, qdot, qddot, tau;
 };
 
-TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointTransformation", "") {
   base_body_id = model->AddBody (0, SpatialTransform(), 
       Joint (
         SpatialVector (0., 0., 0., 1., 0., 0.),
@@ -57,10 +57,10 @@ TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
   Vector3d test_point;
 
   test_point = CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false);
-  CHECK_ARRAY_CLOSE (Vector3d (0., -1., 0.).data(), test_point.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., -1., 0.), AllCloseVector(test_point, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcDynamicFloatingBaseDoubleImplicit", "") {
   // floating base
   base_body_id = model->AddBody (0, SpatialTransform(), 
       Joint (
@@ -88,6 +88,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
   unsigned int i;
+  
   for (i = 0; i < QDDot.size(); i++) {
     LOG << "QDDot[" << i << "] = " << QDDot[i] << endl;
   }
@@ -98,13 +99,10 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
 
   //	std::cout << LogOutput.str() << std::endl;
 
-  CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC);
+  VectorNd target(7); 
-  CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC);
+  target << 0., -9.81, 0., 0., 0., 0., 0.;
-  CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC);
+
-  CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC);
+  REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC);
 
   // We rotate the base... let's see what happens...
   Q[3] = 0.8;
@@ -120,13 +118,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
 
   //	std::cout << LogOutput.str() << std::endl;
 
-  CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC);
+  REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (-9.8100, QDDot[1], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[3], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[6], TEST_PREC);
 
   // We apply a torqe let's see what happens...
   Q[3] = 0.;
@@ -149,16 +141,12 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
 
   //	std::cout << LogOutput.str() << std::endl;
 
-  CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC);
+  target << 0., -8.81, 0., -1., 0., 0., 2.;
-  CHECK_CLOSE (-8.8100, QDDot[1], TEST_PREC);
+
-  CHECK_CLOSE ( 0.0000, QDDot[2], TEST_PREC);
+  REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE (-1.0000, QDDot[3], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[4], TEST_PREC);
-  CHECK_CLOSE ( 0.0000, QDDot[5], TEST_PREC);
-  CHECK_CLOSE ( 2.0000, QDDot[6], TEST_PREC);
 }
 
-TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointVelocityFloatingBaseSimple", "") {
   // floating base
   base_body_id = model->AddBody (0, SpatialTransform(), 
       Joint (
@@ -185,9 +173,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) {
 
   point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
 
-  CHECK_CLOSE(1., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE(0., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(0., point_velocity[2], TEST_PREC);
 
   LOG << "Point velocity = " << point_velocity << endl;
   //	cout << LogOutput.str() << endl;
@@ -200,9 +186,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) {
 
   point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
 
-  CHECK_CLOSE(0., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE(1., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(0., point_velocity[2], TEST_PREC);
 
   LOG << "Point velocity = " << point_velocity << endl;
   //	cout << LogOutput.str() << endl;
@@ -215,15 +199,13 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) {
 
   point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
 
-  CHECK_CLOSE(-1., point_velocity[0], TEST_PREC);
+  REQUIRE_THAT (Vector3d (-1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-  CHECK_CLOSE(0., point_velocity[1], TEST_PREC);
-  CHECK_CLOSE(0., point_velocity[2], TEST_PREC);
   
   LOG << "Point velocity = " << point_velocity << endl;
   //	cout << LogOutput.str() << endl;
 }
 
-TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointVelocityCustom", "") {
   // floating base
   base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));	
   base_body_id = model->AddBody (0, SpatialTransform(), 
@@ -274,7 +256,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) {
       1.093894197498446e+00
       );
 
-  CHECK_ARRAY_CLOSE (point_base_velocity_reference.data(), point_base_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_base_velocity_reference, AllCloseVector(point_base_velocity, TEST_PREC, TEST_PREC));
 }
 
 /** \brief Compares computation of acceleration values for zero qddot
@@ -286,7 +268,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) {
  * Here we omit the term of the generalized acceleration by setting qddot
  * to zero.
  */
-TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointAccelerationNoQDDot", "") {
   // floating base
   base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));	
   base_body_id = model->AddBody (0, SpatialTransform(), 
@@ -364,9 +346,9 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) {
   //	cout << "world_accel = " << point_world_acceleration << endl;
 
 
-  CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_acceleration, AllCloseVector(point_world_acceleration, TEST_PREC, TEST_PREC));
 }
 
 /** \brief Compares computation of acceleration values for zero q and qdot 
@@ -379,7 +361,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) {
  * Here we set q and qdot to zero and only take into account values that
  * are dependent on qddot.
  */
-TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointAccelerationOnlyQDDot", "") {
   // floating base
   base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));	
   base_body_id = model->AddBody (0, SpatialTransform(), 
@@ -447,9 +429,9 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) {
   //	cout << "world_vel   = " << point_world_velocity << endl;
   //	cout << "world_accel = " << point_world_acceleration << endl;
 
-  CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_acceleration, AllCloseVector(point_world_acceleration, TEST_PREC, TEST_PREC));
 }
 
 /** \brief Compares computation of acceleration values for zero q and qdot 
@@ -462,7 +444,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) {
  * Here we set q and qdot to zero and only take into account values that
  * are dependent on qddot.
  */
-TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationFull) {
+TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointAccelerationFull", "") {
   // floating base
   base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));	
   base_body_id = model->AddBody (0, SpatialTransform(), 
@@ -542,9 +524,9 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationFull) {
   //	cout << "world_vel   = " << point_world_velocity << endl;
   //	cout << "world_accel = " << point_world_acceleration << endl;
 
-  CHECK_ARRAY_CLOSE (humans_point_position.data(), point_world_position.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC);
+  REQUIRE_THAT (humans_point_acceleration, AllCloseVector(point_world_acceleration, TEST_PREC, TEST_PREC));
 }
 
 

3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 #include "rbdl/rbdl.h"
 #include <cassert>
 
@@ -12,18 +12,17 @@ using namespace RigidBodyDynamics::Math;
 const double TEST_PREC = 1.0e-11;
 
 // Reduce an angle to the (-pi, pi] range.
-// static double inRange(double angle) {
+static double inRange(double angle) {
-//   while(angle > M_PI) {
+  while(angle > M_PI) {
-//     angle -= 2. * M_PI;
+    angle -= 2. * M_PI;
-//   }
+  }
-//   while(angle <= -M_PI) {
+  while(angle <= -M_PI) {
-//     angle += 2. * M_PI;
+    angle += 2. * M_PI;
-//   }
+  }
-//   return angle;
+  return angle;
-// }
+}
 
-
+TEST_CASE (__FILE__"_ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest", "") {
-TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
   DoublePerpendicularPendulumAbsoluteCoordinates dba
     = DoublePerpendicularPendulumAbsoluteCoordinates();
   DoublePerpendicularPendulumJointCoordinates dbj
@@ -162,12 +161,12 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
 
   //The constraint errors at the position and velocity level
   //must be zero before the accelerations can be tested.
-  for(unsigned int i=0; i<err.rows();++i){
+
-    CHECK_CLOSE(0,err[i],TEST_PREC);
+  VectorNd target1 = VectorNd::Zero(err.rows());
-  }
+  REQUIRE_THAT (target1, AllCloseVector(err, TEST_PREC, TEST_PREC));
-  for(unsigned int i=0; i<errd.rows();++i){
+
-    CHECK_CLOSE(0,errd[i],TEST_PREC);
+  VectorNd target2 = VectorNd::Zero(errd.rows());
-  }
+  REQUIRE_THAT (target2, AllCloseVector(errd, TEST_PREC, TEST_PREC));
 
   //Evaluate the accelerations of the constrained pendulum and
   //compare those to the joint-coordinate pendulum
@@ -187,11 +186,9 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
       CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
                           dba.idB2,Vector3d(dba.l2,0.,0.),true);
 
-  for(unsigned int i=0; i<6;++i){
+  REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
+  REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
+  REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
-  }
 
   ForwardDynamicsConstraintsNullSpace(
         dba.model,dba.q,dba.qd,
@@ -204,12 +201,9 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
   a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
                           dba.idB2,Vector3d(dba.l2,0.,0.),true);
 
-  for(unsigned int i=0; i<6;++i){
+  REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
+  REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
+  REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
-  }
-
 
   ForwardDynamicsConstraintsRangeSpaceSparse(
         dba.model,dba.q,dba.qd,
@@ -221,11 +215,7 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
   a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
                           dba.idB2,Vector3d(dba.l2,0.,0.),true);
 
-  for(unsigned int i=0; i<6;++i){
+  REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
+  REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
+  REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
-    CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
-  }
-
-
 }

3rdparty/rbdl/tests/ImpulsesTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -75,7 +75,7 @@ struct ImpulsesFixture {
   ConstraintSet constraint_set;
 };
 
-TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) {
+TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulse", "") {
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); 
@@ -109,10 +109,10 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) {
   }
 
   // cout << "Point Velocity = " << point_velocity << endl;
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) {
+TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRotated", "") {
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.); 
@@ -152,10 +152,10 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) {
   }
 
   // cout << "Point Velocity = " << point_velocity << endl;
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) {
+TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRotatedCollisionVelocity", "") {
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.); 
   constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.); 
@@ -197,10 +197,10 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) {
   }
 
   // cout << "Point Velocity = " << point_velocity << endl;
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) {
+TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRangeSpaceSparse", "") {
   Q[0] = 0.2;
   Q[1] = -0.5;
   Q[2] = 0.1;
@@ -235,12 +235,11 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) {
   Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
   Vector3d point_velocity_rangespace = CalcPointVelocity (*model, Q, qdot_post_rangespace, contact_body_id, contact_point, true);
 
-  CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_rangespace.data(), qdot_post_direct.rows(), TEST_PREC);
+  REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_rangespace, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_rangespace, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_rangespace.data(), 3, TEST_PREC);
 }
 
-TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) {
+TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseNullSpace", "") {
   Q[0] = 0.2;
   Q[1] = -0.5;
   Q[2] = 0.1;
@@ -275,7 +274,6 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) {
   Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
   Vector3d point_velocity_nullspace = CalcPointVelocity (*model, Q, qdot_post_nullspace, contact_body_id, contact_point, true);
 
-  CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_nullspace.data(), qdot_post_direct.rows(), TEST_PREC);
+  REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_nullspace, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_nullspace, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_nullspace.data(), 3, TEST_PREC);
 }

3rdparty/rbdl/tests/InverseDynamicsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -26,7 +26,7 @@ struct InverseDynamicsFixture {
 };
 
 #ifndef USE_SLOW_SPATIAL_ALGEBRA
-TEST_FIXTURE(InverseDynamicsFixture, TestInverseForwardDynamicsFloatingBase) {
+TEST_CASE_METHOD(InverseDynamicsFixture, __FILE__"_TestInverseForwardDynamicsFloatingBase", "") {
   Body base_body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
 
   model->AddBody (0, SpatialTransform(), 
@@ -71,6 +71,6 @@ TEST_FIXTURE(InverseDynamicsFixture, TestInverseForwardDynamicsFloatingBase) {
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
   InverseDynamics(*model, Q, QDot, QDDot, TauInv);
 
-  CHECK_ARRAY_CLOSE (Tau.data(), TauInv.data(), Tau.size(), TEST_PREC);
+  REQUIRE_THAT (Tau, AllCloseVector(TauInv, TEST_PREC, TEST_PREC));
 }
 #endif

3rdparty/rbdl/tests/InverseKinematicsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -34,7 +34,7 @@ void print_ik_set (const InverseKinematicsConstraintSet &cs) {
 }
 
 /// Checks whether a single point in a 3-link chain can be reached
-TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) {
+TEST_CASE_METHOD (Human36, __FILE__"_ChainSinglePointConstraint", "") {
   q[HipRightRY] = 0.3;
   q[HipRightRX] = 0.3;
   q[HipRightRZ] = 0.3;
@@ -59,14 +59,13 @@ TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) {
     print_ik_set (cs);
   }
 
-  CHECK (result);
+  REQUIRE (result);
 
-  CHECK_CLOSE (0., cs.error_norm, TEST_PREC);
+  REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
 }
 
 
-TEST_FIXTURE ( Human36, ManyPointConstraints ) {
+TEST_CASE_METHOD (Human36, __FILE__"_ManyPointConstraints", "") {
-
   randomizeStates();
 
   Vector3d local_point1 (1., 0., 0.);
@@ -95,9 +94,9 @@ TEST_FIXTURE ( Human36, ManyPointConstraints ) {
 
   bool result = InverseKinematics (*model, q, cs, qres);
 
-  CHECK (result);
+  REQUIRE (result);
 
-  CHECK_CLOSE (0., cs.error_norm, TEST_PREC);
+  REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
 
   UpdateKinematicsCustom (*model, &qres, NULL, NULL);  
   Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1);
@@ -106,16 +105,16 @@ TEST_FIXTURE ( Human36, ManyPointConstraints ) {
   Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4);
   Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5);
 
-  CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position1, AllCloseVector(result_position1, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position2, AllCloseVector(result_position2, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position3, AllCloseVector(result_position3, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position4, AllCloseVector(result_position4, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position5, AllCloseVector(result_position5, TEST_PREC, TEST_PREC)); 
 }
 
 /// Checks whether the end of a 3-link chain can aligned with a given
 // orientation.
-TEST_FIXTURE ( Human36, ChainSingleBodyOrientation ) {
+TEST_CASE_METHOD (Human36, __FILE__"_ChainSingleBodyOrientation", "") {
   q[HipRightRY] = 0.3;
   q[HipRightRX] = 0.3;
   q[HipRightRZ] = 0.3;
@@ -134,11 +133,10 @@ TEST_FIXTURE ( Human36, ChainSingleBodyOrientation ) {
 
   bool result = InverseKinematics (*model, q, cs, qres);
 
-  CHECK (result);
+  REQUIRE (result);
 }
 
-TEST_FIXTURE ( Human36, ManyBodyOrientations ) {
+TEST_CASE_METHOD (Human36, __FILE__"_ManyBodyOrientations", "") {
-
   randomizeStates();
 
   UpdateKinematicsCustom (*model, &q, NULL, NULL);
@@ -161,9 +159,9 @@ TEST_FIXTURE ( Human36, ManyBodyOrientations ) {
 
   bool result = InverseKinematics (*model, q, cs, qres);
 
-  CHECK (result);
+  REQUIRE (result);
 
-  CHECK_CLOSE (0., cs.error_norm, TEST_PREC);
+  REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
 
   UpdateKinematicsCustom (*model, &qres, NULL, NULL);  
   Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false);
@@ -172,14 +170,14 @@ TEST_FIXTURE ( Human36, ManyBodyOrientations ) {
   Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false);
   Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false);
 
-  CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation1, AllCloseMatrix(result_orientation1, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation2, AllCloseMatrix(result_orientation2, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation3, AllCloseMatrix(result_orientation3, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation4, AllCloseMatrix(result_orientation4, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation5, AllCloseMatrix(result_orientation5, TEST_PREC, TEST_PREC)); 
 }
 
-TEST_FIXTURE ( Human36, ChainSingleBodyFullConstraint ) {
+TEST_CASE_METHOD (Human36, __FILE__"_ChainSingleBodyFullConstraint", "") {
   q[HipRightRY] = 0.3;
   q[HipRightRX] = 0.3;
   q[HipRightRZ] = 0.3;
@@ -200,11 +198,10 @@ TEST_FIXTURE ( Human36, ChainSingleBodyFullConstraint ) {
 
   bool result = InverseKinematics (*model, q, cs, qres);
 
-  CHECK (result);
+  REQUIRE (result);
 }
 
-TEST_FIXTURE ( Human36, ManyBodyFullConstraints ) {
+TEST_CASE_METHOD ( Human36, __FILE__"_ManyBodyFullConstraints", "") {
-
   randomizeStates();
 
   Vector3d local_point1 (1., 0., 0.);
@@ -241,9 +238,9 @@ TEST_FIXTURE ( Human36, ManyBodyFullConstraints ) {
 
   bool result = InverseKinematics (*model, q, cs, qres);
 
-  CHECK (result);
+  REQUIRE (result);
 
-  CHECK_CLOSE (0., cs.error_norm, cs.step_tol);
+  REQUIRE_THAT (0., IsClose(cs.error_norm, cs.step_tol, cs.step_tol));
 
   UpdateKinematicsCustom (*model, &qres, NULL, NULL);  
   Matrix3d result_orientation1 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootRight], false);
@@ -258,15 +255,14 @@ TEST_FIXTURE ( Human36, ManyBodyFullConstraints ) {
   Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4);
   Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5);
 
-  CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position1, AllCloseVector(result_position1, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position2, AllCloseVector(result_position2, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position3, AllCloseVector(result_position3, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); 
+  REQUIRE_THAT (target_position4, AllCloseVector(result_position4, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC);
+  REQUIRE_THAT (target_position5, AllCloseVector(result_position5, TEST_PREC, TEST_PREC));
-
+  REQUIRE_THAT (target_orientation1, AllCloseMatrix(result_orientation1, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation2, AllCloseMatrix(result_orientation2, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation3, AllCloseMatrix(result_orientation3, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation4, AllCloseMatrix(result_orientation4, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); 
+  REQUIRE_THAT (target_orientation5, AllCloseMatrix(result_orientation5, TEST_PREC, TEST_PREC)); 
-  CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); 
 }

3rdparty/rbdl/tests/KinematicsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -145,20 +145,20 @@ struct KinematicsFixture6DoF {
 
 
 
-TEST_FIXTURE(KinematicsFixture, TestPositionNeutral) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestPositionNeutral", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
   Vector3d body_position;
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 1., -1.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
 }
 
-TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotated90Deg) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestPositionBaseRotated90Deg", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
 
@@ -168,13 +168,13 @@ TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotated90Deg) {
   Vector3d body_position;
 
   //	cout << LogOutput.str() << endl;
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (-1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (-1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (-1., 1., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (-1., 1., -1.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
 }
 
-TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotatedNeg45Deg) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestPositionBaseRotatedNeg45Deg", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
 
@@ -184,13 +184,13 @@ TEST_FIXTURE(KinematicsFixture, TestPositionBaseRotatedNeg45Deg) {
   Vector3d body_position;
 
   //	cout << LogOutput.str() << endl;
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (0.707106781186547, -0.707106781186547, 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0.707106781186547, -0.707106781186547, 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (sqrt(2.0), 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (sqrt(2.0), 0., -1.), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (sqrt(2.0), 0., -1.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
 }
 
-TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotated90Deg) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestPositionBodyBRotated90Deg", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
   Q[1] = 0.5 * M_PI;
@@ -198,13 +198,13 @@ TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotated90Deg) {
 
   Vector3d body_position;
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
 }
 
-TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotatedNeg45Deg) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestPositionBodyBRotatedNeg45Deg", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
   Q[1] = -0.25 * M_PI;
@@ -212,42 +212,30 @@ TEST_FIXTURE(KinematicsFixture, TestPositionBodyBRotatedNeg45Deg) {
 
   Vector3d body_position;
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
-  CHECK_ARRAY_CLOSE (Vector3d (1 + 0.707106781186547, 1., -0.707106781186547), CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC );
+  REQUIRE_THAT (Vector3d (1 + 0.707106781186547, 1., -0.707106781186547), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_d_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC ));
 }
 
-TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinates) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestCalcBodyToBaseCoordinates", "") {
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (1., 2., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)), TEST_PREC, TEST_PREC));
-      Vector3d (1., 2., 0.),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)),
-      3, TEST_PREC
-      );
 }
 
-TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinatesRotated) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestCalcBodyToBaseCoordinatesRotated", "") {
   Q[2] = 0.5 * M_PI;
 
   // We call ForwardDynamics() as it updates the spatial transformation
   // matrices
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (1., 1., 0.).data(),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false).data(),
-      3, TEST_PREC
-      );
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (0., 1., 0.).data(),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false).data(),
-      3, TEST_PREC
-      );
 
   // Rotate the other way round
   Q[2] = -0.5 * M_PI;
@@ -256,17 +244,9 @@ TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinatesRotated) {
   // matrices
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (1., 1., 0.),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false),
-      3, TEST_PREC
-      );
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (2., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (2., 1., 0.),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false),
-      3, TEST_PREC
-      );
 
   // Rotate around the base
   Q[0] = 0.5 * M_PI;
@@ -276,22 +256,14 @@ TEST_FIXTURE(KinematicsFixture, TestCalcBodyToBaseCoordinatesRotated) {
   // matrices
   ForwardDynamics(*model, Q, QDot, Tau, QDDot);
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (-1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (-1., 1., 0.),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false),
-      3, TEST_PREC
-      );
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (Vector3d (-2., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, TEST_PREC));
-      Vector3d (-2., 1., 0.),
-      CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false),
-      3, TEST_PREC
-      );
 
   //	cout << LogOutput.str() << endl;
 }
 
-TEST(TestCalcPointJacobian) {
+TEST_CASE(__FILE__"_TestCalcPointJacobian", "") {
   Model model;
   Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
 
@@ -335,14 +307,10 @@ TEST(TestCalcPointJacobian) {
 
   point_velocity = G * QDot;
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (point_velocity_ref, AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
-      point_velocity_ref.data(),
-      point_velocity.data(),
-      3, TEST_PREC
-      );
 }
 
-TEST_FIXTURE(KinematicsFixture, TestInverseKinematicSimple) {
+TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestInverseKinematicSimple", "") {
   std::vector<unsigned int> body_ids;
   std::vector<Vector3d> body_points;
   std::vector<Vector3d> target_pos;
@@ -364,17 +332,17 @@ TEST_FIXTURE(KinematicsFixture, TestInverseKinematicSimple) {
   ClearLogOutput();
   bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres);
   //	cout << LogOutput.str() << endl;
-  CHECK_EQUAL (true, res);
+  REQUIRE (true==res);
 
   UpdateKinematicsCustom (*model, &Qres, NULL, NULL);
 
   Vector3d effector;
   effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false);
 
-  CHECK_ARRAY_CLOSE (target.data(), effector.data(), 3, TEST_PREC);	
+  REQUIRE_THAT (target, AllCloseVector(effector, TEST_PREC, TEST_PREC));	
 }
 
-TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicUnreachable) {
+TEST_CASE_METHOD(KinematicsFixture6DoF, __FILE__"_TestInverseKinematicUnreachable", "") {
   std::vector<unsigned int> body_ids;
   std::vector<Vector3d> body_points;
   std::vector<Vector3d> target_pos;
@@ -396,17 +364,17 @@ TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicUnreachable) {
   ClearLogOutput();
   bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-8, 0.9, 1000);
   //	cout << LogOutput.str() << endl;
-  CHECK_EQUAL (true, res);
+  REQUIRE (true==res);
 
   UpdateKinematicsCustom (*model, &Qres, NULL, NULL);
 
   Vector3d effector;
   effector = CalcBodyToBaseCoordinates(*model, Qres, body_id, body_point, false);
 
-  CHECK_ARRAY_CLOSE (Vector3d (2.0, 0., 0.).data(), effector.data(), 3, 1.0e-7);	
+  REQUIRE_THAT (Vector3d (2.0, 0., 0.), AllCloseVector(effector, 1.0e-7, 1.0e-7));	
 }
 
-TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicTwoPoints) {
+TEST_CASE_METHOD(KinematicsFixture6DoF, __FILE__"_TestInverseKinematicTwoPoints", "") {
   std::vector<unsigned int> body_ids;
   std::vector<Vector3d> body_points;
   std::vector<Vector3d> target_pos;
@@ -431,7 +399,7 @@ TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicTwoPoints) {
 
   ClearLogOutput();
   bool res = InverseKinematics (*model, Q, body_ids, body_points, target_pos, Qres, 1.0e-3, 0.9, 200);
-  CHECK_EQUAL (true, res);
+  REQUIRE (true==res);
 
   //	cout << LogOutput.str() << endl;
   UpdateKinematicsCustom (*model, &Qres, NULL, NULL);
@@ -440,13 +408,13 @@ TEST_FIXTURE(KinematicsFixture6DoF, TestInverseKinematicTwoPoints) {
 
   // testing with very low precision
   effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[0], body_points[0], false);
-  CHECK_ARRAY_CLOSE (target_pos[0].data(), effector.data(), 3, 1.0e-1);	
+  REQUIRE_THAT (target_pos[0], AllCloseVector(effector, 1.0e-1, 1.0e-1));	
 
   effector = CalcBodyToBaseCoordinates(*model, Qres, body_ids[1], body_points[1], false);
-  CHECK_ARRAY_CLOSE (target_pos[1].data(), effector.data(), 3, 1.0e-1);	
+  REQUIRE_THAT (target_pos[1], AllCloseVector(effector, 1.0e-1, 1.0e-1));	
 }
 
-TEST ( FixedJointBodyCalcBodyToBase ) {
+TEST_CASE (__FILE__"_FixedJointBodyCalcBodyToBase", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -461,10 +429,10 @@ TEST ( FixedJointBodyCalcBodyToBase ) {
   VectorNd Q_zero = VectorNd::Zero (model.dof_count);
   Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 1., 0.1));
 
-  CHECK_ARRAY_CLOSE (Vector3d (1., 2., 0.1).data(), base_coords.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 2., 0.1), AllCloseVector(base_coords, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointBodyCalcBodyToBaseRotated ) {
+TEST_CASE (__FILE__"_FixedJointBodyCalcBodyToBaseRotated", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -483,10 +451,10 @@ TEST ( FixedJointBodyCalcBodyToBaseRotated ) {
   Vector3d base_coords = CalcBodyToBaseCoordinates (model, Q, fixed_body_id, Vector3d (1., 0., 0.));
   //	cout << LogOutput.str() << endl;	
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), base_coords.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 2., 0.), AllCloseVector(base_coords, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointBodyCalcBaseToBody ) {
+TEST_CASE (__FILE__"_FixedJointBodyCalcBaseToBody", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -501,10 +469,10 @@ TEST ( FixedJointBodyCalcBaseToBody ) {
   VectorNd Q_zero = VectorNd::Zero (model.dof_count);
   Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q_zero, fixed_body_id, Vector3d (1., 2., 0.1));
 
-  CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.1).data(), base_coords.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 1., 0.1), AllCloseVector(base_coords, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointBodyCalcBaseToBodyRotated ) {
+TEST_CASE (__FILE__"_FixedJointBodyCalcBaseToBodyRotated", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -523,10 +491,10 @@ TEST ( FixedJointBodyCalcBaseToBodyRotated ) {
   Vector3d base_coords = CalcBaseToBodyCoordinates (model, Q, fixed_body_id, Vector3d (0., 2., 0.));
   // cout << LogOutput.str() << endl;	
 
-  CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.).data(), base_coords.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(base_coords, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointBodyWorldOrientation ) {
+TEST_CASE (__FILE__"_FixedJointBodyWorldOrientation", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -545,10 +513,10 @@ TEST ( FixedJointBodyWorldOrientation ) {
 
   Matrix3d reference = transform.E;
 
-  CHECK_ARRAY_CLOSE (reference.data(), orientation.data(), 9, TEST_PREC);
+  REQUIRE_THAT (reference, AllCloseMatrix(orientation, TEST_PREC, TEST_PREC));
 }
 
-TEST ( FixedJointCalcPointJacobian ) {
+TEST_CASE (__FILE__"_FixedJointCalcPointJacobian", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -575,10 +543,10 @@ TEST ( FixedJointCalcPointJacobian ) {
   Vector3d point_velocity_jacobian = G * QDot;
   Vector3d point_velocity_reference = CalcPointVelocity (model, Q, QDot, fixed_body_id, point_position);
 
-  CHECK_ARRAY_CLOSE (point_velocity_reference.data(), point_velocity_jacobian.data(), 3, TEST_PREC);
+  REQUIRE_THAT (point_velocity_reference, AllCloseVector(point_velocity_jacobian, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, SpatialJacobianSimple ) {
+TEST_CASE_METHOD (Human36, __FILE__"_SpatialJacobianSimple", "") {
   randomizeStates();
 
   unsigned int foot_r_id = model->GetBodyId ("foot_r");
@@ -589,10 +557,10 @@ TEST_FIXTURE ( Human36, SpatialJacobianSimple ) {
   UpdateKinematicsCustom (*model, &q, &qdot, NULL);
   SpatialVector v_body = SpatialVector(G * qdot);
 
-  CHECK_ARRAY_CLOSE (model->v[foot_r_id].data(), v_body.data(), 6, TEST_PREC);
+  REQUIRE_THAT (model->v[foot_r_id], AllCloseVector(v_body, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, SpatialJacobianFixedBody ) {
+TEST_CASE_METHOD (Human36, __FILE__"_SpatialJacobianFixedBody", "") {
   randomizeStates();
 
   unsigned int uppertrunk_id = model->GetBodyId ("uppertrunk");
@@ -608,10 +576,10 @@ TEST_FIXTURE ( Human36, SpatialJacobianFixedBody ) {
 
   SpatialVector v_fixed_body = model->mFixedBodies[fixed_body_id].mParentTransform.apply (model->v[movable_parent]);
 
-  CHECK_ARRAY_CLOSE (v_fixed_body.data(), v_body.data(), 6, TEST_PREC);
+  REQUIRE_THAT (v_fixed_body, AllCloseVector(v_body, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, CalcPointJacobian6D ) {
+TEST_CASE_METHOD (Human36, __FILE__"_CalcPointJacobian6D", "") {
   randomizeStates();
 
   unsigned int foot_r_id = model->GetBodyId ("foot_r");
@@ -629,10 +597,10 @@ TEST_FIXTURE ( Human36, CalcPointJacobian6D ) {
   UpdateKinematicsCustom (*model, &q, &qdot, NULL);
   SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id]));
 
-  CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0_jac.data(), 6, TEST_PREC);
+  REQUIRE_THAT (v_foot_0_ref, AllCloseVector(v_foot_0_jac, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, CalcPointVelocity6D ) {
+TEST_CASE_METHOD (Human36, __FILE__"_CalcPointVelocity6D", "") {
   randomizeStates();
 
   unsigned int foot_r_id = model->GetBodyId ("foot_r");
@@ -648,10 +616,10 @@ TEST_FIXTURE ( Human36, CalcPointVelocity6D ) {
   UpdateKinematicsCustom (*model, &q, &qdot, NULL);
   SpatialVector v_foot_0_ref = X_foot.apply(model->X_base[foot_r_id].inverse().apply(model->v[foot_r_id]));
 
-  CHECK_ARRAY_CLOSE (v_foot_0_ref.data(), v_foot_0.data(), 6, TEST_PREC);
+  REQUIRE_THAT (v_foot_0_ref, AllCloseVector(v_foot_0, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, CalcPointAcceleration6D ) {
+TEST_CASE_METHOD (Human36, __FILE__"_CalcPointAcceleration6D", "") {
   randomizeStates();
 
   unsigned int foot_r_id = model->GetBodyId ("foot_r");
@@ -676,5 +644,5 @@ TEST_FIXTURE ( Human36, CalcPointAcceleration6D ) {
         )
       );
 
-  CHECK_ARRAY_CLOSE (a_foot_0_ref.data(), a_foot_0.data(), 6, TEST_PREC);
+  REQUIRE_THAT (a_foot_0_ref, AllCloseVector(a_foot_0, TEST_PREC, TEST_PREC));
 }

3rdparty/rbdl/tests/MathTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include "rbdl/Logging.h"
 #include "rbdl/rbdl_math.h"
@@ -10,10 +10,12 @@ const double TEST_PREC = 1.0e-14;
 using namespace std;
 using namespace RigidBodyDynamics::Math;
 
+using namespace Catch::Matchers::Floating;
+
 struct MathFixture {
 };
 
-TEST (GaussElimPivot) {
+TEST_CASE(__FILE__"_GaussElimPivot", "") {
   ClearLogOutput();
 
   MatrixNd A;
@@ -37,7 +39,7 @@ TEST (GaussElimPivot) {
 
   LinSolveGaussElimPivot (A, b, x);
 
-  CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
+  REQUIRE_THAT (test_result, AllCloseVector(x));
 
   A(0,0) = 0; A(0,1) = -2; A(0,2) = 1;
   A(1,0) = 1; A(1,1) =  1; A(1,2) = 5;
@@ -48,62 +50,30 @@ TEST (GaussElimPivot) {
   test_result[1] = 1;
   test_result[2] = 3;
 
-  CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
+  x[0] += 1.0e-13;
+
+  REQUIRE_THAT (test_result, AllCloseVector(x));
 }
 
-TEST (Dynamic_1D_initialize_value) {
+TEST_CASE(__FILE__"_QuaternionSlerpNegativeCosHalfTheta", "") {
-  VectorNd myvector_10 = VectorNd::Constant ((size_t) 10, 12.);
+	ClearLogOutput();
 
-  double *test_values = new double[10];
+	Quaternion q1 (-0.518934,0.561432,-0.074923,0.640225);
-  for (unsigned int i = 0; i < 10; i++)
+	Quaternion q2 (0.54702,-0.564195,0.078871,-0.613379);
-    test_values[i] = 12.;
 
-  CHECK_ARRAY_EQUAL (test_values, myvector_10.data(), 10);
+	Quaternion s = q1.slerp (0.2021, q2);
-  delete[] test_values;
+        Quaternion q_ref (0.0068865, 0.406762, -0.000610507, 0.913655);
+
+        REQUIRE_THAT (s, AllCloseVector(q_ref));
 }
 
-TEST (Dynamic_2D_initialize_value) {
+TEST_CASE(__FILE__"_QuaternionFromMatrixSingularity", "") {
-  MatrixNd mymatrix_10x10 = MatrixNd::Constant (10, 10, 12.);
+	ClearLogOutput();
+	Matrix3d m;
+	m << -1., 0, 0, 0, 1, 0, 0, 0, -1;
 
-  double *test_values = new double[10 * 10];
+	Quaternion q = Quaternion::fromMatrix(m);
-  for (unsigned int i = 0; i < 10; i++)
+        Quaternion q_ref (0., 1., 0., 0.);
-    for (unsigned int j = 0; j < 10; j++)
-      test_values[i*10 + j] = 12.;
 
-  CHECK_ARRAY_EQUAL (test_values, mymatrix_10x10.data(), 10*10);
+        REQUIRE_THAT (q, AllCloseVector(q_ref));
-  delete[] test_values;
-}
-
-TEST (SpatialMatrix_Multiplication) {
-  SpatialMatrix X_1 (
-      1.,  2.,  3.,  4.,  5.,  6.,
-      11., 12., 13., 14., 15., 16.,
-      21., 22., 23., 24., 25., 26.,
-      31., 32., 33., 34., 35., 36.,
-      41., 42., 43., 44., 45., 46.,
-      51., 52., 53., 54., 55., 56.
-      );
-
-  SpatialMatrix X_2 (X_1);
-
-  X_2 *= 2.;
-
-  SpatialMatrix correct_result (
-      1442,    1484,    1526,    1568,    1610,    1652,
-      4562,    4724,    4886,    5048,    5210,    5372,
-      7682,    7964,    8246,    8528,    8810,    9092,
-      10802,   11204,   11606,   12008,   12410,   12812,
-      13922,   14444,   14966,   15488,   16010,   16532,
-      17042,   17684,   18326,   18968,   19610,   20252
-      );
-
-  SpatialMatrix test_result = X_1 * X_2;
-
-  CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC);
-
-  // check the *= operator:
-  test_result = X_1;
-  test_result *= X_2;
-
-  CHECK_ARRAY_CLOSE (correct_result.data(), test_result.data(), 6 * 6, TEST_PREC);
 }

3rdparty/rbdl/tests/ModelTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -28,117 +28,117 @@ struct ModelFixture {
   Model *model;
 };
 
-TEST_FIXTURE(ModelFixture, TestInit) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestInit", "") {
-  CHECK_EQUAL (1u, model->lambda.size());
+  REQUIRE (1u == model->lambda.size());
-  CHECK_EQUAL (1u, model->mu.size());
+  REQUIRE (1u == model->mu.size());
-  CHECK_EQUAL (0u, model->dof_count);
+  REQUIRE (0u == model->dof_count);
 
-  CHECK_EQUAL (0u, model->q_size);
+  REQUIRE (0u == model->q_size);
-  CHECK_EQUAL (0u, model->qdot_size);
+  REQUIRE (0u == model->qdot_size);
 
-  CHECK_EQUAL (1u, model->v.size());
+  REQUIRE (1u == model->v.size());
-  CHECK_EQUAL (1u, model->a.size());
+  REQUIRE (1u == model->a.size());
 
-  CHECK_EQUAL (1u, model->mJoints.size());
+  REQUIRE (1u == model->mJoints.size());
-  CHECK_EQUAL (1u, model->S.size());
+  REQUIRE (1u == model->S.size());
 
-  CHECK_EQUAL (1u, model->c.size());
+  REQUIRE (1u == model->c.size());
-  CHECK_EQUAL (1u, model->IA.size());
+  REQUIRE (1u == model->IA.size());
-  CHECK_EQUAL (1u, model->pA.size());
+  REQUIRE (1u == model->pA.size());
-  CHECK_EQUAL (1u, model->U.size());
+  REQUIRE (1u == model->U.size());
-  CHECK_EQUAL (1u, model->d.size());
+  REQUIRE (1u == model->d.size());
-  CHECK_EQUAL (1u, model->u.size());
+  REQUIRE (1u == model->u.size());
-  CHECK_EQUAL (1u, model->Ic.size());
+  REQUIRE (1u == model->Ic.size());
-  CHECK_EQUAL (1u, model->I.size());
+  REQUIRE (1u == model->I.size());
 
-  CHECK_EQUAL (1u, model->X_lambda.size());
+  REQUIRE (1u == model->X_lambda.size());
-  CHECK_EQUAL (1u, model->X_base.size());
+  REQUIRE (1u == model->X_base.size());
-  CHECK_EQUAL (1u, model->mBodies.size());
+  REQUIRE (1u == model->mBodies.size());
 }
 
-TEST_FIXTURE(ModelFixture, TestAddBodyDimensions) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestAddBodyDimensions", "") {
   Body body;
   Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
   unsigned int body_id = 0;
   body_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
 
-  CHECK_EQUAL (1u, body_id);
+  REQUIRE (1u == body_id);
-  CHECK_EQUAL (2u, model->lambda.size());
+  REQUIRE (2u == model->lambda.size());
-  CHECK_EQUAL (2u, model->mu.size());
+  REQUIRE (2u == model->mu.size());
-  CHECK_EQUAL (1u, model->dof_count);
+  REQUIRE (1u == model->dof_count);
 
-  CHECK_EQUAL (2u, model->v.size());
+  REQUIRE (2u == model->v.size());
-  CHECK_EQUAL (2u, model->a.size());
+  REQUIRE (2u == model->a.size());
 
-  CHECK_EQUAL (2u, model->mJoints.size());
+  REQUIRE (2u == model->mJoints.size());
-  CHECK_EQUAL (2u, model->S.size());
+  REQUIRE (2u == model->S.size());
 
-  CHECK_EQUAL (2u, model->c.size());
+  REQUIRE (2u == model->c.size());
-  CHECK_EQUAL (2u, model->IA.size());
+  REQUIRE (2u == model->IA.size());
-  CHECK_EQUAL (2u, model->pA.size());
+  REQUIRE (2u == model->pA.size());
-  CHECK_EQUAL (2u, model->U.size());
+  REQUIRE (2u == model->U.size());
-  CHECK_EQUAL (2u, model->d.size());
+  REQUIRE (2u == model->d.size());
-  CHECK_EQUAL (2u, model->u.size());
+  REQUIRE (2u == model->u.size());
-  CHECK_EQUAL (2u, model->Ic.size());
+  REQUIRE (2u == model->Ic.size());
-  CHECK_EQUAL (2u, model->I.size());
+  REQUIRE (2u == model->I.size());
 
   SpatialVector spatial_zero;
   spatial_zero.setZero();
 
-  CHECK_EQUAL (2u, model->X_lambda.size());
+  REQUIRE (2u == model->X_lambda.size());
-  CHECK_EQUAL (2u, model->X_base.size());
+  REQUIRE (2u == model->X_base.size());
-  CHECK_EQUAL (2u, model->mBodies.size());
+  REQUIRE (2u == model->mBodies.size());
 }
 
-TEST_FIXTURE(ModelFixture, TestFloatingBodyDimensions) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestFloatingBodyDimensions", "") {
   Body body;
   Joint float_base_joint (JointTypeFloatingBase);
 
   model->AppendBody (SpatialTransform(), float_base_joint, body);
 
-  CHECK_EQUAL (3u, model->lambda.size());
+  REQUIRE (3u == model->lambda.size());
-  CHECK_EQUAL (3u, model->mu.size());
+  REQUIRE (3u == model->mu.size());
-  CHECK_EQUAL (6u, model->dof_count);
+  REQUIRE (6u == model->dof_count);
-  CHECK_EQUAL (7u, model->q_size);
+  REQUIRE (7u == model->q_size);
-  CHECK_EQUAL (6u, model->qdot_size);
+  REQUIRE (6u == model->qdot_size);
 
-  CHECK_EQUAL (3u, model->v.size());
+  REQUIRE (3u == model->v.size());
-  CHECK_EQUAL (3u, model->a.size());
+  REQUIRE (3u == model->a.size());
 
-  CHECK_EQUAL (3u, model->mJoints.size());
+  REQUIRE (3u == model->mJoints.size());
-  CHECK_EQUAL (3u, model->S.size());
+  REQUIRE (3u == model->S.size());
 
-  CHECK_EQUAL (3u, model->c.size());
+  REQUIRE (3u == model->c.size());
-  CHECK_EQUAL (3u, model->IA.size());
+  REQUIRE (3u == model->IA.size());
-  CHECK_EQUAL (3u, model->pA.size());
+  REQUIRE (3u == model->pA.size());
-  CHECK_EQUAL (3u, model->U.size());
+  REQUIRE (3u == model->U.size());
-  CHECK_EQUAL (3u, model->d.size());
+  REQUIRE (3u == model->d.size());
-  CHECK_EQUAL (3u, model->u.size());
+  REQUIRE (3u == model->u.size());
 
   SpatialVector spatial_zero;
   spatial_zero.setZero();
 
-  CHECK_EQUAL (3u, model->X_lambda.size());
+  REQUIRE (3u == model->X_lambda.size());
-  CHECK_EQUAL (3u, model->X_base.size());
+  REQUIRE (3u == model->X_base.size());
-  CHECK_EQUAL (3u, model->mBodies.size());
+  REQUIRE (3u == model->mBodies.size());
 }
 
 /** \brief Tests whether the joint and body information stored in the Model are computed correctly
 */
-TEST_FIXTURE(ModelFixture, TestAddBodySpatialValues) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestAddBodySpatialValues", "") {
   Body body;
   Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
   model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body);
 
   SpatialVector spatial_joint_axis(0., 0., 1., 0., 0., 0.);
-  CHECK_EQUAL (spatial_joint_axis, joint.mJointAxes[0]);
+  REQUIRE (spatial_joint_axis == joint.mJointAxes[0]);
 
   // \Todo: Dynamic properties
 }
 
-TEST_FIXTURE(ModelFixture, TestAddBodyTestBodyName) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestAddBodyTestBodyName", "") {
   Body body;
   Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -146,11 +146,11 @@ TEST_FIXTURE(ModelFixture, TestAddBodyTestBodyName) {
 
   unsigned int body_id = model->GetBodyId("mybody");
 
-  CHECK_EQUAL (1u, body_id);
+  REQUIRE (1u == body_id);
-  CHECK_EQUAL (std::numeric_limits<unsigned int>::max(), model->GetBodyId("unknownbody"));
+  REQUIRE (std::numeric_limits<unsigned int>::max() == model->GetBodyId("unknownbody"));
 }
 
-TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
+TEST_CASE_METHOD(ModelFixture, __FILE__"_TestjcalcSimple", "") {
   Body body;
   Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
 
@@ -177,8 +177,8 @@ TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
       0., 0., 1., 0., 0., 0.
       );
 
-  CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16));
+  REQUIRE_THAT (test_vector, AllCloseVector(model->v_J[1], 1.0e-16, 1.0e-16));
-  CHECK_EQUAL (test_joint_axis, model->S[1]);
+  REQUIRE (test_joint_axis == model->S[1]);
 
   Q[0] = M_PI * 0.5;
   QDot[0] = 1.;
@@ -193,11 +193,11 @@ TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
       0.,  0.,  0., -1.,  0.,  0.,
       0.,  0.,  0.,  0.,  0.,  1.;
 
-  CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16));
+  REQUIRE_THAT (test_vector, AllCloseVector(model->v_J[1], 1.0e-16, 1.0e-16));
-  CHECK_EQUAL (test_joint_axis, model->S[1]);
+  REQUIRE (test_joint_axis == model->S[1]);
 }
 
-TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
+TEST_CASE_METHOD (ModelFixture, __FILE__"_TestTransformBaseToLocal", "") {
   Body body;
 
   unsigned int body_id = model->AddBody (0, SpatialTransform(),
@@ -224,7 +224,7 @@ TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
   body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false);
   base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false);
 
-  CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC);
+  REQUIRE_THAT (base_coords, AllCloseVector(base_coords_back, TEST_PREC, TEST_PREC));
 
   q[0] = 1.;
   q[1] = 0.2;
@@ -237,10 +237,10 @@ TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
   body_coords = CalcBaseToBodyCoordinates (*model, q, body_id, base_coords, false);
   base_coords_back = CalcBodyToBaseCoordinates (*model, q, body_id, body_coords, false);
 
-  CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC);
+  REQUIRE_THAT (base_coords, AllCloseVector(base_coords_back, TEST_PREC, TEST_PREC));
 }
 
-TEST ( Model2DoFJoint ) {
+TEST_CASE (__FILE__"_Model2DoFJoint", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -274,10 +274,10 @@ TEST ( Model2DoFJoint ) {
   ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
   ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
 
-  CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+  REQUIRE_THAT (QDDot_std, AllCloseVector(QDDot_2, TEST_PREC, TEST_PREC));
 }
 
-TEST ( Model3DoFJoint ) {
+TEST_CASE (__FILE__"_Model3DoFJoint", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -326,10 +326,10 @@ TEST ( Model3DoFJoint ) {
   ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
   ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
 
-  CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+  REQUIRE_THAT (QDDot_std, AllCloseVector(QDDot_2, TEST_PREC, TEST_PREC));
 }
 
-TEST ( Model6DoFJoint ) {
+TEST_CASE (__FILE__"_Model6DoFJoint", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -379,15 +379,15 @@ TEST ( Model6DoFJoint ) {
   VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
   VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
 
-  assert (model_std.q_size == model_2.q_size);
+  REQUIRE  (model_std.q_size == model_2.q_size);
 
   ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
   ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
 
-  CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+  REQUIRE_THAT (QDDot_std, AllCloseVector(QDDot_2, TEST_PREC, TEST_PREC));
 }
 
-TEST ( ModelFixedJointQueryBodyId ) {
+TEST_CASE (__FILE__"_ModelFixedJointQueryBodyId", "" ) {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -400,7 +400,7 @@ TEST ( ModelFixedJointQueryBodyId ) {
   model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
   unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
 
-  CHECK_EQUAL (fixed_body_id, model.GetBodyId("fixed_body"));
+  REQUIRE (fixed_body_id == model.GetBodyId("fixed_body"));
 }
 
 /*
@@ -408,7 +408,7 @@ TEST ( ModelFixedJointQueryBodyId ) {
  * newly added parent is actually the moving body that the fixed body is
  * attached to.
  */
-TEST ( ModelAppendToFixedBody ) {
+TEST_CASE (__FILE__"_ModelAppendToFixedBody", "") {
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -421,12 +421,12 @@ TEST ( ModelAppendToFixedBody ) {
   //	unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
   unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
 
-  CHECK_EQUAL (movable_body + 1, appended_body_id);
+  REQUIRE (movable_body + 1 == appended_body_id);
-  CHECK_EQUAL (movable_body, model.lambda[appended_body_id]);
+  REQUIRE (movable_body == model.lambda[appended_body_id]);
 }
 
 // Adds a fixed body to another fixed body.
-TEST ( ModelAppendFixedToFixedBody ) {
+TEST_CASE (__FILE__"_ModelAppendFixedToFixedBody", "") {
   Body null_body;
 
   double movable_mass = 1.1;
@@ -449,22 +449,22 @@ TEST ( ModelAppendFixedToFixedBody ) {
   unsigned int fixed_body_2_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body_2");
   unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
 
-  CHECK_EQUAL (movable_body + 1, appended_body_id);
+  REQUIRE (movable_body + 1 == appended_body_id);
-  CHECK_EQUAL (movable_body, model.lambda[appended_body_id]);
+  REQUIRE (movable_body == model.lambda[appended_body_id]);
-  CHECK_EQUAL (movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass);
+  REQUIRE (movable_mass + fixed_mass * 2. == model.mBodies[movable_body].mMass);
 
-  CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent);
+  REQUIRE (movable_body == model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent);
-  CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent);
+  REQUIRE (movable_body == model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent);
 
   double new_mass = 3.5;
   Vector3d new_com = (1. / new_mass) * (movable_mass * movable_com + fixed_mass * (fixed_com + fixed_displacement) + fixed_mass * (fixed_com + fixed_displacement * 2.));
 
-  CHECK_ARRAY_CLOSE (new_com.data(), model.mBodies[movable_body].mCenterOfMass.data(), 3, TEST_PREC);
+  REQUIRE_THAT (new_com, AllCloseVector(model.mBodies[movable_body].mCenterOfMass, TEST_PREC, TEST_PREC));
 }
 
 // Ensures that the transformations of the movable parent and fixed joint
 // frame is in proper order
-TEST ( ModelFixedJointRotationOrderTranslationRotation ) {
+TEST_CASE (__FILE__"_ModelFixedJointRotationOrderTranslationRotation", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -485,12 +485,12 @@ TEST ( ModelFixedJointRotationOrderTranslationRotation ) {
   Q[0] = 45 * M_PI / 180.;
   Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.));
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.).data(), point.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(point, TEST_PREC, TEST_PREC));
 }
 
 // Ensures that the transformations of the movable parent and fixed joint
 // frame is in proper order
-TEST ( ModelFixedJointRotationOrderRotationTranslation ) {
+TEST_CASE (__FILE__"_ModelFixedJointRotationOrderRotationTranslation", "") {
   // the standard modeling using a null body
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -511,10 +511,10 @@ TEST ( ModelFixedJointRotationOrderRotationTranslation ) {
   Q[0] = 45 * M_PI / 180.;
   Vector3d point = CalcBodyToBaseCoordinates (model, Q, body_after_fixed, Vector3d (0., 1., 0.));
 
-  CHECK_ARRAY_CLOSE (Vector3d (-1., 2., 0.).data(), point.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (-1., 2., 0.), AllCloseVector(point, TEST_PREC, TEST_PREC));
 }
 
-TEST ( ModelGetBodyName ) {
+TEST_CASE (__FILE__"_ModelGetBodyName", "") {
   Body null_body;
   Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
   Body fixed_body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
@@ -527,38 +527,38 @@ TEST ( ModelGetBodyName ) {
   unsigned int fixed_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), Joint(JointTypeFixed), fixed_body, "fixed_body");
   unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
 
-  CHECK_EQUAL (string("fixed_body"), model.GetBodyName(fixed_body_id));
+  REQUIRE (string("fixed_body") == model.GetBodyName(fixed_body_id));
-  CHECK_EQUAL (string("appended_body"), model.GetBodyName(appended_body_id));
+  REQUIRE (string("appended_body") == model.GetBodyName(appended_body_id));
-  CHECK_EQUAL (string(""), model.GetBodyName(123));
+  REQUIRE (string("") == model.GetBodyName(123));
 }
 
-TEST_FIXTURE ( RotZRotZYXFixed, ModelGetParentBodyId ) {
+TEST_CASE_METHOD (RotZRotZYXFixed, __FILE__"_ModelGetParentBodyId", "") {
-  CHECK_EQUAL (0u, model->GetParentBodyId(0));
+  REQUIRE (0u == model->GetParentBodyId(0));
-  CHECK_EQUAL (0u, model->GetParentBodyId(body_a_id));
+  REQUIRE (0u == model->GetParentBodyId(body_a_id));
-  CHECK_EQUAL (body_a_id, model->GetParentBodyId(body_b_id));
+  REQUIRE (body_a_id == model->GetParentBodyId(body_b_id));
 }
 
-TEST_FIXTURE(RotZRotZYXFixed, ModelGetParentIdFixed) {
+TEST_CASE_METHOD(RotZRotZYXFixed, __FILE__"_ModelGetParentIdFixed", "") {
-  CHECK_EQUAL (body_b_id, model->GetParentBodyId(body_fixed_id));
+  REQUIRE (body_b_id == model->GetParentBodyId(body_fixed_id));
 }
 
-TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrame) {
+TEST_CASE_METHOD(RotZRotZYXFixed, __FILE__"_ModelGetJointFrame", "") {
   SpatialTransform transform_a = model->GetJointFrame (body_a_id);
   SpatialTransform transform_b = model->GetJointFrame (body_b_id);
   SpatialTransform transform_root = model->GetJointFrame (0);
 
-  CHECK_ARRAY_EQUAL (fixture_transform_a.r.data(), transform_a.r.data(), 3);
+  REQUIRE_THAT (fixture_transform_a.r, AllCloseVector(transform_a.r, 0., 0.));
-  CHECK_ARRAY_EQUAL (fixture_transform_b.r.data(), transform_b.r.data(), 3);
+  REQUIRE_THAT (fixture_transform_b.r, AllCloseVector(transform_b.r, 0., 0.));
-  CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3);
+  REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(transform_root.r, 0., 0.));
 }
 
-TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrameFixed) {
+TEST_CASE_METHOD(RotZRotZYXFixed, __FILE__"_ModelGetJointFrameFixed", "") {
   SpatialTransform transform_fixed = model->GetJointFrame (body_fixed_id);
 
-  CHECK_ARRAY_EQUAL (fixture_transform_fixed.r.data(), transform_fixed.r.data(), 3);
+  REQUIRE_THAT (fixture_transform_fixed.r, AllCloseVector(transform_fixed.r, 0., 0.));
 }
 
-TEST_FIXTURE(RotZRotZYXFixed, ModelSetJointFrame) {
+TEST_CASE_METHOD(RotZRotZYXFixed, __FILE__"_ModelSetJointFrame", "") {
   SpatialTransform new_transform_a = Xtrans (Vector3d(-1., -2., -3.));
   SpatialTransform new_transform_b = Xtrans (Vector3d(-4., -5., -6.));
   SpatialTransform new_transform_root = Xtrans (Vector3d(-99, -99., -99.));
@@ -571,12 +571,12 @@ TEST_FIXTURE(RotZRotZYXFixed, ModelSetJointFrame) {
   SpatialTransform transform_b = model->GetJointFrame (body_b_id);
   SpatialTransform transform_root = model->GetJointFrame (0);
 
-  CHECK_ARRAY_EQUAL (new_transform_a.r.data(), transform_a.r.data(), 3);
+  REQUIRE_THAT (new_transform_a.r, AllCloseVector(transform_a.r, 0., 0.));
-  CHECK_ARRAY_EQUAL (new_transform_b.r.data(), transform_b.r.data(), 3);
+  REQUIRE_THAT (new_transform_b.r, AllCloseVector(transform_b.r, 0., 0.));
-  CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3);
+  REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(transform_root.r, 0., 0.));
 }
 
-TEST (CalcBodyWorldOrientationFixedJoint) {
+TEST_CASE (__FILE__"_CalcBodyWorldOrientationFixedJoint", "") {
   Model model_fixed;
   Model model_movable;
 
@@ -596,10 +596,10 @@ TEST (CalcBodyWorldOrientationFixedJoint) {
   Matrix3d E_fixed = CalcBodyWorldOrientation (model_fixed, q_fixed, body_id_fixed);
   Matrix3d E_movable = CalcBodyWorldOrientation (model_movable, q_movable, body_id_movable);
 
-  CHECK_ARRAY_CLOSE (E_movable.data(), E_fixed.data(), 9, TEST_PREC);
+  REQUIRE_THAT (E_movable, AllCloseMatrix(E_fixed, TEST_PREC, TEST_PREC));
 }
 
-TEST (TestAddFixedBodyToRoot) {
+TEST_CASE (__FILE__"_TestAddFixedBodyToRoot", "") {
   Model model;
 
   Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.));
@@ -614,19 +614,18 @@ TEST (TestAddFixedBodyToRoot) {
   // Add a mobile boby
   unsigned int movable_body = model.AppendBody (Xrotx (45 * M_PI / 180), JointTypeRevoluteX, body, "MovableBody");
 
-  CHECK_EQUAL ((unsigned int) 2, model.mBodies.size());
+  REQUIRE (2 == model.mBodies.size());
-  CHECK_EQUAL ((unsigned int) 2, model.mFixedBodies.size());
+  REQUIRE (2 == model.mFixedBodies.size());
 
   VectorNd q = VectorNd::Zero(model.q_size);
 
   Vector3d base_coords = CalcBodyToBaseCoordinates(model, q, body_id_fixed, Vector3d::Zero());
-  CHECK_ARRAY_EQUAL(Vector3d (1., 0., 0.).data(), base_coords.data(), 3);
+  REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(base_coords, 0., 0.));
 
   base_coords = CalcBodyToBaseCoordinates(model, q, body_id_fixed2, Vector3d::Zero());
-  CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3);
+  REQUIRE_THAT (Vector3d (2., 0., 0.), AllCloseVector(base_coords, 0., 0.));
 
   base_coords = CalcBodyToBaseCoordinates(model, q, movable_body, Vector3d::Zero());
-  CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3);
+  REQUIRE_THAT (Vector3d (2., 0., 0.), AllCloseVector(base_coords, 0., 0.));
-
 }
 

3rdparty/rbdl/tests/MultiDofTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -122,7 +122,7 @@ void ConvertQAndQDotFromEmulated (
   }
 }
 
-TEST(TestQuaternionIntegration ) {
+TEST_CASE(__FILE__"_TestQuaternionIntegration", "") {
   double timestep = 0.001;
 
   Vector3d zyx_angles_t0 (0.1, 0.2, 0.3);
@@ -141,23 +141,23 @@ TEST(TestQuaternionIntegration ) {
   //   B) integration under the assumption that the angular velocity is
   //   constant
   // However I am not entirely sure about this...
-  CHECK_ARRAY_CLOSE (q_zyx_t1.data(), q_t1.data(), 4, 1.0e-5);
+  REQUIRE_THAT(q_zyx_t1, AllCloseVector(q_t1, 1.0e-5, 1.0e-5));
 }
 
-TEST_FIXTURE(SphericalJoint, TestQIndices) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestQIndices", "") {
-  CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index);
+  REQUIRE(0u == multdof3_model.mJoints[1].q_index);
-  CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index);
+  REQUIRE(1u == multdof3_model.mJoints[2].q_index);
-  CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index);
+  REQUIRE(4u == multdof3_model.mJoints[3].q_index);
 
-  CHECK_EQUAL (5u, emulated_model.q_size);
+  REQUIRE(5u == emulated_model.q_size);
-  CHECK_EQUAL (5u, emulated_model.qdot_size);
+  REQUIRE(5u == emulated_model.qdot_size);
 
-  CHECK_EQUAL (6u, multdof3_model.q_size);
+  REQUIRE(6u == multdof3_model.q_size);
-  CHECK_EQUAL (5u, multdof3_model.qdot_size);
+  REQUIRE(5u == multdof3_model.qdot_size);
-  CHECK_EQUAL (5u, multdof3_model.multdof3_w_index[2]);
+  REQUIRE(5u == multdof3_model.multdof3_w_index[2]);
 }
 
-TEST_FIXTURE(SphericalJoint, TestGetQuaternion) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestGetQuaternion", "") {
   multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body);
 
   sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size);
@@ -165,16 +165,16 @@ TEST_FIXTURE(SphericalJoint, TestGetQuaternion) {
   sphQDDot = VectorNd::Zero ((size_t) multdof3_model.qdot_size);
   sphTau = VectorNd::Zero ((size_t) multdof3_model.qdot_size);
 
-  CHECK_EQUAL (10u, multdof3_model.q_size);
+  REQUIRE (10u == multdof3_model.q_size);
-  CHECK_EQUAL (8u, multdof3_model.qdot_size);
+  REQUIRE (8u == multdof3_model.qdot_size);
 
-  CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index);
+  REQUIRE (0u == multdof3_model.mJoints[1].q_index);
-  CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index);
+  REQUIRE (1u == multdof3_model.mJoints[2].q_index);
-  CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index);
+  REQUIRE (4u == multdof3_model.mJoints[3].q_index);
-  CHECK_EQUAL (5u, multdof3_model.mJoints[4].q_index);
+  REQUIRE (5u == multdof3_model.mJoints[4].q_index);
 
-  CHECK_EQUAL (8u, multdof3_model.multdof3_w_index[2]);
+  REQUIRE (8u == multdof3_model.multdof3_w_index[2]);
-  CHECK_EQUAL (9u, multdof3_model.multdof3_w_index[4]);
+  REQUIRE (9u == multdof3_model.multdof3_w_index[4]);
 
   sphQ[0] = 100.;
   sphQ[1] = 0.;
@@ -189,14 +189,14 @@ TEST_FIXTURE(SphericalJoint, TestGetQuaternion) {
 
   Quaternion reference_1 (0., 1., 2., 4.);
   Quaternion quat_1 = multdof3_model.GetQuaternion (2, sphQ);
-  CHECK_ARRAY_EQUAL (reference_1.data(), quat_1.data(), 4);
+  REQUIRE_THAT (reference_1, AllCloseVector(quat_1));
 
   Quaternion reference_3 (-6., -7., -8., -9.);
   Quaternion quat_3 = multdof3_model.GetQuaternion (4, sphQ);
-  CHECK_ARRAY_EQUAL (reference_3.data(), quat_3.data(), 4);
+  REQUIRE_THAT (reference_3, AllCloseVector(quat_3));
 }
 
-TEST_FIXTURE(SphericalJoint, TestSetQuaternion) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestSetQuaternion", "") {
   multdof3_model.AppendBody (Xtrans (Vector3d (1., 0., 0.)), joint_spherical, body);
 
   sphQ = VectorNd::Zero ((size_t) multdof3_model.q_size);
@@ -207,15 +207,15 @@ TEST_FIXTURE(SphericalJoint, TestSetQuaternion) {
   Quaternion reference_1 (0., 1., 2., 3.);
   multdof3_model.SetQuaternion (2, reference_1, sphQ);
   Quaternion test = multdof3_model.GetQuaternion (2, sphQ);
-  CHECK_ARRAY_EQUAL (reference_1.data(), test.data(), 4);
+  REQUIRE_THAT (reference_1, AllCloseVector(test));
 
   Quaternion reference_2 (11., 22., 33., 44.);
   multdof3_model.SetQuaternion (4, reference_2, sphQ);
   test = multdof3_model.GetQuaternion (4, sphQ);
-  CHECK_ARRAY_EQUAL (reference_2.data(), test.data(), 4);
+  REQUIRE_THAT (reference_2, AllCloseVector(test));
 }
 
-TEST_FIXTURE(SphericalJoint, TestOrientation) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestOrientation", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.1;
   emuQ[2] = 1.1;
@@ -233,10 +233,10 @@ TEST_FIXTURE(SphericalJoint, TestOrientation) {
   Matrix3d emu_orientation = CalcBodyWorldOrientation (emulated_model, emuQ, emu_child_id);
   Matrix3d sph_orientation = CalcBodyWorldOrientation (multdof3_model, sphQ, sph_child_id);
 
-  CHECK_ARRAY_CLOSE (emu_orientation.data(), sph_orientation.data(), 9, TEST_PREC);
+  REQUIRE_THAT(emu_orientation, AllCloseMatrix(sph_orientation, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestUpdateKinematics) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestUpdateKinematics", "") {
   emuQ[0] = 1.;
   emuQ[1] = 1.;
   emuQ[2] = 1.;
@@ -273,16 +273,16 @@ TEST_FIXTURE(SphericalJoint, TestUpdateKinematics) {
   UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, &emuQDDot);
   UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, &sphQDDot);
 
-  CHECK_ARRAY_CLOSE (emulated_model.v[emu_body_id].data(), multdof3_model.v[sph_body_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.v[emu_body_id], AllCloseVector(multdof3_model.v[sph_body_id], TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (emulated_model.a[emu_body_id].data(), multdof3_model.a[sph_body_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.a[emu_body_id], AllCloseVector(multdof3_model.a[sph_body_id], TEST_PREC, TEST_PREC));
 
   UpdateKinematics (multdof3_model, sphQ, sphQDot, sphQDDot);
 
-  CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.v[emu_child_id], AllCloseVector(multdof3_model.v[sph_child_id], TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.a[emu_child_id], AllCloseVector(multdof3_model.a[sph_child_id], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestSpatialVelocities) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestSpatialVelocities", "") {
   emuQ[0] = 1.;
   emuQ[1] = 2.;
   emuQ[2] = 3.;
@@ -298,10 +298,10 @@ TEST_FIXTURE(SphericalJoint, TestSpatialVelocities) {
   UpdateKinematicsCustom (emulated_model, &emuQ, &emuQDot, NULL);
   UpdateKinematicsCustom (multdof3_model, &sphQ, &sphQDot, NULL);
 
-  CHECK_ARRAY_CLOSE (emulated_model.v[emu_child_id].data(), multdof3_model.v[sph_child_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.v[emu_child_id], AllCloseVector(multdof3_model.v[sph_child_id], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestForwardDynamicsQAndQDot) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestForwardDynamicsQAndQDot", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -317,10 +317,10 @@ TEST_FIXTURE(SphericalJoint, TestForwardDynamicsQAndQDot) {
   ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, emuQDDot);
   ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, sphQDDot);
 
-  CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[sph_child_id].data(), 6, TEST_PREC);
+  REQUIRE_THAT (emulated_model.a[emu_child_id], AllCloseVector(multdof3_model.a[sph_child_id], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestDynamicsConsistencyRNEA_ABA ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestDynamicsConsistencyRNEA_ABA", "" ) {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -346,10 +346,10 @@ TEST_FIXTURE(SphericalJoint, TestDynamicsConsistencyRNEA_ABA ) {
   VectorNd tau_id (VectorNd::Zero (multdof3_model.qdot_size));
   InverseDynamics (multdof3_model, sphQ, sphQDot, sphQDDot, tau_id);
 
-  CHECK_ARRAY_CLOSE (sphTau.data(), tau_id.data(), tau_id.size(), TEST_PREC);
+  REQUIRE_THAT (sphTau, AllCloseVector(tau_id, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestCRBA ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestCRBA", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -396,10 +396,10 @@ TEST_FIXTURE(SphericalJoint, TestCRBA ) {
     H_id.block(0, i, multdof3_model.qdot_size, 1) = H_col;
   }
 
-  CHECK_ARRAY_CLOSE (H_id.data(), H_crba.data(), multdof3_model.qdot_size * multdof3_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (H_id, AllCloseMatrix(H_crba, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestForwardDynamicsLagrangianVsABA ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestForwardDynamicsLagrangianVsABA", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -426,10 +426,10 @@ TEST_FIXTURE(SphericalJoint, TestForwardDynamicsLagrangianVsABA ) {
   ForwardDynamicsLagrangian (multdof3_model, sphQ, sphQDot, sphTau, QDDot_lag);
   ForwardDynamics (multdof3_model, sphQ, sphQDot, sphTau, QDDot_aba);
 
-  CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), multdof3_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (QDDot_lag, AllCloseVector(QDDot_aba, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestContactsLagrangian) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestContactsLagrangian", "") {
   ConstraintSet constraint_set_emu;
 
   constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.));
@@ -451,10 +451,10 @@ TEST_FIXTURE(SphericalJoint, TestContactsLagrangian) {
   ForwardDynamicsConstraintsDirect (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot);
   VectorNd sph_force_lagrangian = constraint_set_sph.force;
 
-  CHECK_ARRAY_CLOSE (emu_force_lagrangian.data(), sph_force_lagrangian.data(), 3, TEST_PREC);
+  REQUIRE_THAT (emu_force_lagrangian, AllCloseVector(sph_force_lagrangian, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestContacts) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestContacts", "") {
   ConstraintSet constraint_set_emu;
 
   constraint_set_emu.AddContactConstraint (emu_child_id, Vector3d (0., 0., -1.), Vector3d (1., 0., 0.));
@@ -476,10 +476,10 @@ TEST_FIXTURE(SphericalJoint, TestContacts) {
   ForwardDynamicsContactsKokkevis (multdof3_model, sphQ, sphQDot, sphTau, constraint_set_sph, sphQDDot);
   VectorNd sph_force_kokkevis = constraint_set_sph.force;
 
-  CHECK_ARRAY_CLOSE (emu_force_kokkevis.data(), sph_force_kokkevis.data(), 3, TEST_PREC);
+  REQUIRE_THAT (emu_force_kokkevis, AllCloseVector(sph_force_kokkevis, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedLagrangian ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulatedLagrangian", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -504,10 +504,10 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedLagrangian ) {
   ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu);
   ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedArticulatedBodyAlgorithm ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulatedArticulatedBodyAlgorithm", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -532,10 +532,10 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedArticulatedBodyAlgorithm ) {
   ForwardDynamics (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu);
   ForwardDynamics (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedContacts ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulatedContacts", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -572,22 +572,22 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedContacts ) {
   ForwardDynamicsConstraintsDirect (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
   ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC, TEST_PREC));
 
   ClearLogOutput();
 
   ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
   ForwardDynamicsContactsKokkevis (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm());
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC * QDDot_emu.norm(), TEST_PREC * QDDot_emu.norm()));
 
   ForwardDynamicsContactsKokkevis (emulated_model, emuQ, emuQDot, emuTau, CS_emulated, QDDot_emu);
   ForwardDynamicsConstraintsDirect (eulerzyx_model, emuQ, emuQDot, emuTau, CS_euler, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC * QDDot_emu.norm());
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC * QDDot_emu.norm(), TEST_PREC * QDDot_emu.norm()));
 }
 
-TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedCRBA ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulatedCRBA", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -600,10 +600,10 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulatedCRBA ) {
   CompositeRigidBodyAlgorithm (emulated_model, emuQ, H_emulated);
   CompositeRigidBodyAlgorithm (eulerzyx_model, emuQ, H_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_eulerzyx.data(), emulated_model.q_size * emulated_model.q_size, TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_eulerzyx, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestJointTypeTranslationZYX ) {
+TEST_CASE (__FILE__"_TestJointTypeTranslationZYX", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -636,12 +636,12 @@ TEST ( TestJointTypeTranslationZYX ) {
   VectorNd id_3dof(tau);
   InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated);
   InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof);
-  CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm());
+  REQUIRE_THAT (id_emulated, AllCloseVector(id_3dof, TEST_PREC * id_emulated.norm(), TEST_PREC * id_emulated.norm()));
 
   ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated);
   ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof);
 
-  CHECK_ARRAY_EQUAL (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof));
 
   MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size()));
   MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size()));
@@ -649,10 +649,10 @@ TEST ( TestJointTypeTranslationZYX ) {
   CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated);
   CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestJointTypeEulerXYZ ) {
+TEST_CASE (__FILE__"_TestJointTypeEulerXYZ", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -684,13 +684,13 @@ TEST ( TestJointTypeEulerXYZ ) {
   UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated);
   UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated);
 
-  CHECK_ARRAY_EQUAL (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9);
+  REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E));
-  CHECK_ARRAY_EQUAL (model_emulated.v[3].data(), model_3dof.v[1].data(), 6);
+  REQUIRE_THAT (model_emulated.v[3], AllCloseVector(model_3dof.v[1]));
 
   ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated);
   ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof);
 
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC);
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC, TEST_PREC));
 
   MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size()));
   MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size()));
@@ -698,10 +698,10 @@ TEST ( TestJointTypeEulerXYZ ) {
   CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated);
   CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestJointTypeEulerYXZ ) {
+TEST_CASE (__FILE__"_TestJointTypeEulerYXZ", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -734,20 +734,20 @@ TEST ( TestJointTypeEulerYXZ ) {
   UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated);
   UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated);
 
-  CHECK_ARRAY_CLOSE (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9, TEST_PREC);
+  REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC);
+  REQUIRE_THAT (model_emulated.v[3], AllCloseVector(model_3dof.v[1], TEST_PREC, TEST_PREC));
 
   VectorNd id_emulated (tau);
   VectorNd id_3dof(tau);
   InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated);
   InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof);
 
-  CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC);
+  REQUIRE_THAT (id_emulated, AllCloseVector(id_3dof, TEST_PREC, TEST_PREC));
 
   ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated);
   ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof);
 
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC);
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC, TEST_PREC));
 
   MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size()));
   MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size()));
@@ -755,10 +755,10 @@ TEST ( TestJointTypeEulerYXZ ) {
   CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated);
   CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestJointTypeEulerZXY ) {
+TEST_CASE (__FILE__"_TestJointTypeEulerZXY", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -793,20 +793,20 @@ TEST ( TestJointTypeEulerZXY ) {
   UpdateKinematicsCustom (model_emulated, &q, &qdot, &qddot_emulated);
   UpdateKinematicsCustom (model_3dof, &q, &qdot, &qddot_emulated);
 
-  CHECK_ARRAY_CLOSE (model_emulated.X_base[3].E.data(), model_3dof.X_base[1].E.data(), 9, TEST_PREC);
+  REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E, TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC);
+  REQUIRE_THAT (model_emulated.v[3], AllCloseVector(model_3dof.v[1], TEST_PREC, TEST_PREC));
 
   VectorNd id_emulated (tau);
   VectorNd id_3dof(tau);
   InverseDynamics (model_emulated, q, qdot, qddot_emulated, id_emulated);
   InverseDynamics (model_3dof, q, qdot, qddot_emulated, id_3dof);
 
-  CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC);
+  REQUIRE_THAT (id_emulated, AllCloseVector(id_3dof, TEST_PREC, TEST_PREC));
 
   ForwardDynamicsLagrangian (model_emulated, q, qdot, tau, qddot_emulated);
   ForwardDynamicsLagrangian (model_3dof, q, qdot, tau, qddot_3dof);
 
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC);
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC, TEST_PREC));
 
   MatrixNd H_emulated (MatrixNd::Zero (q.size(), q.size()));
   MatrixNd H_3dof (MatrixNd::Zero (q.size(), q.size()));
@@ -818,10 +818,10 @@ TEST ( TestJointTypeEulerZXY ) {
   CompositeRigidBodyAlgorithm (model_emulated, q, H_emulated);
   CompositeRigidBodyAlgorithm (model_3dof, q, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (Human36, TestUpdateKinematics) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestUpdateKinematics2", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -835,12 +835,12 @@ TEST_FIXTURE (Human36, TestUpdateKinematics) {
   UpdateKinematics (*model_emulated, q, qdot, qddot);
   UpdateKinematics (*model_3dof, q, qdot, qddot);
 
-  CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyPelvis]].data(), model_3dof->v[body_id_3dof[BodyPelvis]].data(), 6, TEST_PREC);
+  REQUIRE_THAT (model_emulated->v[body_id_emulated[BodyPelvis]], AllCloseVector(model_3dof->v[body_id_3dof[BodyPelvis]], TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyThighRight]].data(), model_3dof->v[body_id_3dof[BodyThighRight]].data(), 6, TEST_PREC);
+  REQUIRE_THAT (model_emulated->v[body_id_emulated[BodyThighRight]], AllCloseVector(model_3dof->v[body_id_3dof[BodyThighRight]], TEST_PREC, TEST_PREC));
-  CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyShankRight]].data(), model_3dof->v[body_id_3dof[BodyShankRight]].data(), 6, TEST_PREC);
+  REQUIRE_THAT (model_emulated->v[body_id_emulated[BodyShankRight]], AllCloseVector(model_3dof->v[body_id_3dof[BodyShankRight]], TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (Human36, TestInverseDynamics) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestInverseDynamics", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -855,10 +855,10 @@ TEST_FIXTURE (Human36, TestInverseDynamics) {
   InverseDynamics (*model_emulated, q, qdot, qddot, id_emulated);
   InverseDynamics (*model_3dof, q, qdot, qddot, id_3dof);
 
-  CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm());
+  REQUIRE_THAT (id_emulated, AllCloseVector(id_3dof, TEST_PREC * id_emulated.norm(), TEST_PREC * id_emulated.norm()));
 }
 
-TEST_FIXTURE (Human36, TestNonlinearEffects) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestNonlinearEffects", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -873,10 +873,10 @@ TEST_FIXTURE (Human36, TestNonlinearEffects) {
   NonlinearEffects (*model_emulated, q, qdot, nle_emulated);
   NonlinearEffects (*model_3dof, q, qdot, nle_3dof);
 
-  CHECK_ARRAY_CLOSE (nle_emulated.data(), nle_3dof.data(), nle_emulated.size(), TEST_PREC * nle_emulated.norm());
+  REQUIRE_THAT (nle_emulated, AllCloseVector(nle_3dof, TEST_PREC * nle_emulated.norm(), TEST_PREC * nle_emulated.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianKokkevis", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -888,19 +888,19 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) {
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
   ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_kokkevis);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
   ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_kokkevis);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
   ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_kokkevis);
 
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianSparse", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -912,18 +912,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) {
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_sparse);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_sparse);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_sparse);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianNullSpace", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -935,18 +935,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) {
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
   ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace);
-  CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
+  REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofLagrangian", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -956,18 +956,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) {
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
   ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
   ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 
   ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
   ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofSparse", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -977,22 +977,22 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) {
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
 
   for (unsigned int i = 0; i < q.size(); i++) {
-    CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
+    REQUIRE (model_emulated->lambda_q[i]==model_3dof->lambda_q[i]);
   }
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
-  CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
+  REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofKokkevisSparse", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -1002,7 +1002,7 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
 
   for (unsigned int i = 0; i < q.size(); i++) {
-    CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
+    REQUIRE (model_emulated->lambda_q[i]==model_3dof->lambda_q[i]);
   }
 
   VectorNd qddot_sparse (qddot_emulated);
@@ -1010,18 +1010,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_sparse);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
-  CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
+  REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_sparse);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
-  CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
+  REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
 
   ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_sparse);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
-  CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
+  REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
 }
 
-TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) {
+TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofKokkevisMultiple", "") {
   for (unsigned int i = 0; i < q.size(); i++) {
     q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
     qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@@ -1033,18 +1033,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) {
 
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis_2);
-  CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
+  REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
 
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis_2);
-  CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
+  REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
 
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
   ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis_2);
-  CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
+  REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
 }
 
-TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) {
+TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulated", "") {
   emuQ[0] = 1.1;
   emuQ[1] = 1.2;
   emuQ[2] = 1.3;
@@ -1069,10 +1069,10 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) {
   ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu);
   ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx);
 
-  CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC);
+  REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE ( Human36, SolveMInvTimesTau) {
+TEST_CASE_METHOD (Human36, __FILE__"_SolveMInvTimesTau", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     q[i] = rand() / static_cast<double>(RAND_MAX);
     tau[i] = rand() / static_cast<double>(RAND_MAX);
@@ -1086,10 +1086,10 @@ TEST_FIXTURE ( Human36, SolveMInvTimesTau) {
   VectorNd qddot_minv (q);
   CalcMInvTimesTau (*model, q, tau, qddot_minv);
 
-  CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_minv.norm());
+  REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv,  TEST_PREC * qddot_minv.norm(), TEST_PREC * qddot_minv.norm()));
 }
 
-TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) {
+TEST_CASE_METHOD (Human36, __FILE__"_SolveMInvTimesTauReuse", "") {
   for (unsigned int i = 0; i < model->dof_count; i++) {
     q[i] = rand() / static_cast<double>(RAND_MAX);
     tau[i] = rand() / static_cast<double>(RAND_MAX);
@@ -1113,6 +1113,6 @@ TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) {
 
     CalcMInvTimesTau (*model, q, tau, qddot_minv);
 
-    CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_solve_llt.norm());
+    REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv,  TEST_PREC * qddot_solve_llt.norm(), TEST_PREC * qddot_solve_llt.norm()));
   }
 }

3rdparty/rbdl/tests/ScrewJointTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 #include <cmath>
@@ -60,16 +60,16 @@ struct ScrewJoint1DofFixedBase {
 };
 
 
-TEST_FIXTURE ( ScrewJoint1DofFixedBase, UpdateKinematics ) {
+TEST_CASE_METHOD (ScrewJoint1DofFixedBase, __FILE__"_UpdateKinematics", "") {
   q[0] = 1;
   qdot[0] = 2;
   qddot[0] = 0;
 
   UpdateKinematics (*model, q, qdot, qddot);
 
-  CHECK_ARRAY_EQUAL (Xrot(1,Vector3d(0,0,1)).E.data(), model->X_base[roller].E.data(), 9);
+  REQUIRE_THAT (Xrot(1,Vector3d(0,0,1)).E, AllCloseMatrix(model->X_base[roller].E));
-  CHECK_ARRAY_EQUAL (Vector3d(1.,0.,0.).data(), model->X_base[roller].r.data(), 3);
+  REQUIRE_THAT (Vector3d(1.,0.,0.), AllCloseVector(model->X_base[roller].r));
-  CHECK_ARRAY_EQUAL (SpatialVector(0.,0.,2.,cos(q[0])*2,-sin(q[0])*2.,0.).data(), model->v[roller].data(), 6);
+  REQUIRE_THAT (SpatialVector(0.,0.,2.,cos(q[0])*2,-sin(q[0])*2.,0.), AllCloseVector(model->v[roller]));
 
   SpatialVector a0(model->a[roller]);
   SpatialVector v0(model->v[roller]);
@@ -83,11 +83,10 @@ TEST_FIXTURE ( ScrewJoint1DofFixedBase, UpdateKinematics ) {
   v0 = model->v[roller] - v0;
   v0 /= epsilon;
   
-  CHECK_ARRAY_CLOSE (a0.data(),v0.data(), 6, 1e-5); //finite diff vs. analytical derivative
+  REQUIRE_THAT (a0, AllCloseVector(v0, 1e-5, 1e-5)); //finite diff vs. analytical derivative
-
 }
 
-TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) {
+TEST_CASE_METHOD (ScrewJoint1DofFixedBase, __FILE__"_Jacobians", "") {
   q[0] = 1;
   qdot[0] = 0;
   qddot[0] = 9;
@@ -99,7 +98,8 @@ TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) {
 
   refPtBaseCoord = CalcBodyToBaseCoordinates(*model, q, roller, refPt);
 
-  CHECK_ARRAY_EQUAL (Vector3d(1+cos(1), sin(1), 3).data(), refPtBaseCoord.data(), 3);
+
+  REQUIRE_THAT (Vector3d(1+cos(1), sin(1), 3), AllCloseVector(refPtBaseCoord));
   
   CalcPointJacobian(*model, q, roller, refPt, GrefPt);
 
@@ -107,5 +107,5 @@ TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) {
   Gexpected(1,0) = cos(1);
   Gexpected(2,0) = 0;
 
-  CHECK_ARRAY_EQUAL (Gexpected.data(), GrefPt.data(), 3);
+  REQUIRE_THAT (Gexpected, AllCloseMatrix(GrefPt));
 }

3rdparty/rbdl/tests/SparseFactorizationTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -17,7 +17,7 @@ using namespace RigidBodyDynamics::Math;
 
 const double TEST_PREC = 1.0e-12;
 
-TEST_FIXTURE (FloatingBase12DoF, TestSparseFactorizationLTL) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestSparseFactorizationLTL", "") {
   for (unsigned int i = 0; i < model->q_size; i++) {
     Q[i] = static_cast<double> (i + 1) * 0.1;
   }
@@ -30,10 +30,10 @@ TEST_FIXTURE (FloatingBase12DoF, TestSparseFactorizationLTL) {
   SparseFactorizeLTL (*model, L);
   MatrixNd LTL = L.transpose() * L;
 
-  CHECK_ARRAY_CLOSE (H.data(), LTL.data(), model->qdot_size * model->qdot_size, TEST_PREC);
+  REQUIRE_THAT (H, AllCloseMatrix(LTL, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLx) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestSparseSolveLx", "") {
   for (unsigned int i = 0; i < model->q_size; i++) {
     Q[i] = static_cast<double> (i + 1) * 0.1;
   }
@@ -48,10 +48,10 @@ TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLx) {
 
   SparseSolveLx (*model, L, x);
 
-  CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC);
+  REQUIRE_THAT (Q, AllCloseVector(x, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLTx) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestSparseSolveLTx", "") {
   for (unsigned int i = 0; i < model->q_size; i++) {
     Q[i] = static_cast<double> (i + 1) * 0.1;
   }
@@ -66,10 +66,10 @@ TEST_FIXTURE (FloatingBase12DoF, TestSparseSolveLTx) {
 
   SparseSolveLTx (*model, L, x);
 
-  CHECK_ARRAY_CLOSE (Q.data(), x.data(), model->qdot_size, TEST_PREC);
+  REQUIRE_THAT (Q, AllCloseVector(x, TEST_PREC, TEST_PREC));
 }
 
-TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsSparse) {
+TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_ForwardDynamicsContactsSparse", "") {
   ConstraintSet constraint_set_var1;
 
   constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
@@ -108,10 +108,10 @@ TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsSparse) {
   ClearLogOutput();
   ForwardDynamicsConstraintsRangeSpaceSparse (*model, Q, QDot, Tau, constraint_set_var1, QDDot_var1);
 
-  CHECK_ARRAY_CLOSE (QDDot.data(), QDDot_var1.data(), QDDot.size(), TEST_PREC);
+  REQUIRE_THAT (QDDot, AllCloseVector(QDDot_var1, TEST_PREC, TEST_PREC));
 }
 
-TEST ( TestSparseFactorizationMultiDof) {
+TEST_CASE (__FILE__"_TestSparseFactorizationMultiDof", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -171,24 +171,24 @@ TEST ( TestSparseFactorizationMultiDof) {
   SparseFactorizeLTL (model_emulated, H_emulated);
   SparseFactorizeLTL (model_3dof, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 
   x_emulated = b;
   SparseSolveLx (model_emulated, H_emulated, x_emulated);
   x_3dof = b;
   SparseSolveLx (model_3dof, H_3dof, x_3dof);
 
-  CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9);
+  REQUIRE_THAT (x_emulated, AllCloseVector(x_3dof, 1.0e-9, 1.0e-9));
 
   x_emulated = b;
   SparseSolveLTx (model_emulated, H_emulated, x_emulated);
   x_3dof = b;
   SparseSolveLTx (model_3dof, H_3dof, x_3dof);
 
-  CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9);
+  REQUIRE_THAT (x_emulated, AllCloseVector(x_3dof, 1.0e-9, 1.0e-9));
 }
 
-TEST ( TestSparseFactorizationMultiDofAndFixed) {
+TEST_CASE (__FILE__"_TestSparseFactorizationMultiDofAndFixed", "") {
   Model model_emulated;
   Model model_3dof;
 
@@ -250,19 +250,19 @@ TEST ( TestSparseFactorizationMultiDofAndFixed) {
   SparseFactorizeLTL (model_emulated, H_emulated);
   SparseFactorizeLTL (model_3dof, H_3dof);
 
-  CHECK_ARRAY_CLOSE (H_emulated.data(), H_3dof.data(), q.size() * q.size(), TEST_PREC);
+  REQUIRE_THAT (H_emulated, AllCloseMatrix(H_3dof, TEST_PREC, TEST_PREC));
 
   x_emulated = b;
   SparseSolveLx (model_emulated, H_emulated, x_emulated);
   x_3dof = b;
   SparseSolveLx (model_3dof, H_3dof, x_3dof);
 
-  CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9);
+  REQUIRE_THAT (x_emulated, AllCloseVector(x_3dof, 1.0e-9, 1.0e-9));
 
   x_emulated = b;
   SparseSolveLTx (model_emulated, H_emulated, x_emulated);
   x_3dof = b;
   SparseSolveLTx (model_3dof, H_3dof, x_3dof);
 
-  CHECK_ARRAY_CLOSE (x_emulated.data(), x_3dof.data(), x_emulated.size(), 1.0e-9);
+  REQUIRE_THAT (x_emulated, AllCloseVector(x_3dof, 1.0e-9, 1.0e-9));
 }

3rdparty/rbdl/tests/SpatialAlgebraTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 #include <iomanip>
@@ -38,7 +38,7 @@ Vector3d get_translation (const SpatialMatrix &m) {
 }
 
 /// \brief Checks the multiplication of a SpatialMatrix with a SpatialVector
-TEST(TestSpatialMatrixTimesSpatialVector) {
+TEST_CASE(__FILE__"_TestSpatialMatrixTimesSpatialVector", "") {
   SpatialMatrix s_matrix (
       1., 0., 0., 0., 0., 7.,
       0., 2., 0., 0., 8., 0.,
@@ -57,11 +57,12 @@ TEST(TestSpatialMatrixTimesSpatialVector) {
   SpatialVector test_result (
       43., 44., 45., 34., 35., 40.
       );
-  CHECK_EQUAL (test_result, result);
+  REQUIRE_THAT(test_result, AllCloseVector(result, 0., 0.));
 }
 
+
 /// \brief Checks the multiplication of a scalar with a SpatialVector
-TEST(TestScalarTimesSpatialVector) {
+TEST_CASE(__FILE__"_TestScalarTimesSpatialVector", "") {
   SpatialVector s_vector (
       1., 2., 3., 4., 5., 6.
       );
@@ -71,11 +72,12 @@ TEST(TestScalarTimesSpatialVector) {
 
   SpatialVector test_result(3., 6., 9., 12., 15., 18.);
 
-  CHECK_EQUAL (test_result, result);
+  REQUIRE_THAT (test_result, AllCloseVector(result, 0., 0.));
 }
 
+
 /// \brief Checks the multiplication of a scalar with a SpatialMatrix
-TEST(TestScalarTimesSpatialMatrix) {
+TEST_CASE(__FILE__"_TestScalarTimesSpatialMatrix", "") {
   SpatialMatrix s_matrix (
       1., 0., 0., 0., 0., 7.,
       0., 2., 0., 0., 8., 0.,
@@ -97,11 +99,12 @@ TEST(TestScalarTimesSpatialMatrix) {
       12., 0., 0., 0., 0., 18.
       );
 
-  CHECK_EQUAL (test_result, result);
+  REQUIRE_THAT (test_result, AllCloseMatrix(result, 0., 0.));
 }
 
+
 /// \brief Checks the multiplication of a scalar with a SpatialMatrix
-TEST(TestSpatialMatrixTimesSpatialMatrix) {
+TEST_CASE(__FILE__"_TestSpatialMatrixTimesSpatialMatrix", "") {
   SpatialMatrix s_matrix (
       1., 0., 0., 0., 0., 7.,
       0., 2., 0., 0., 8., 0.,
@@ -123,14 +126,14 @@ TEST(TestSpatialMatrixTimesSpatialMatrix) {
       28., 0., 0., 0., 0., 64.
       );
 
-  CHECK_EQUAL (test_result, result);
+  REQUIRE_THAT(test_result, AllCloseMatrix(result, 0., 0.));
 }
 
 /// \brief Checks the adjoint method
 //
 // This method computes a spatial force transformation from a spatial
 // motion transformation and vice versa
-TEST(TestSpatialMatrixTransformAdjoint) {
+TEST_CASE(__FILE__"_TestSpatialMatrixTransformAdjoint", "") {
   SpatialMatrix s_matrix (
       1.,  2.,  3.,  4.,  5.,  6.,
       7.,  8.,  9., 10., 11., 12.,
@@ -150,10 +153,10 @@ TEST(TestSpatialMatrixTransformAdjoint) {
       10., 11., 12., 28., 29., 30.,
       16., 17., 18., 34., 35., 36.);
 
-  CHECK_EQUAL (test_result_matrix, result);
+  REQUIRE_THAT (test_result_matrix, AllCloseMatrix(result, 0., 0.));
 }
 
-TEST(TestSpatialMatrixInverse) {
+TEST_CASE(__FILE__"_TestSpatialMatrixInverse", "") {
   SpatialMatrix s_matrix (
       0, 1, 2, 0, 1, 2,
       3, 4, 5, 3, 4, 5,
@@ -172,10 +175,10 @@ TEST(TestSpatialMatrixInverse) {
       2, 5, 8, 2, 5, 8
       );
 
-  CHECK_EQUAL (test_inv, spatial_inverse(s_matrix));
+  REQUIRE_THAT (test_inv, AllCloseMatrix(spatial_inverse(s_matrix), 0., 0.));
 }
 
-TEST(TestSpatialMatrixGetRotation) {
+TEST_CASE(__FILE__"_TestSpatialMatrixGetRotation", "") {
   SpatialMatrix spatial_transform (
       1.,  2.,  3.,  0.,  0.,  0.,
       4.,  5.,  6.,  0.,  0.,  0.,
@@ -193,10 +196,10 @@ TEST(TestSpatialMatrixGetRotation) {
       7., 8., 9.
       );
 
-  CHECK_EQUAL(test_result, rotation);
+  REQUIRE_THAT (test_result, AllCloseMatrix(rotation, 0., 0.));
 }
 
-TEST(TestSpatialMatrixGetTranslation) {
+TEST_CASE(__FILE__"_TestSpatialMatrixGetTranslation", "") {
   SpatialMatrix spatial_transform (
       0.,  0.,  0.,  0.,  0.,  0.,
       0.,  0.,  0.,  0.,  0.,  0.,
@@ -211,10 +214,10 @@ TEST(TestSpatialMatrixGetTranslation) {
       1., 2., 3.
       );
 
-  CHECK_EQUAL( test_result, translation);
+  REQUIRE_THAT (test_result, AllCloseMatrix(translation, 0., 0.));
 }
 
-TEST(TestSpatialVectorCross) {
+TEST_CASE(__FILE__"_TestSpatialVectorCross", "") {
   SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
 
   SpatialMatrix test_cross (
@@ -227,15 +230,15 @@ TEST(TestSpatialVectorCross) {
       );
 
   SpatialMatrix s_vec_cross (crossm(s_vec));
-  CHECK_EQUAL (test_cross, s_vec_cross);
+  REQUIRE_THAT (test_cross, AllCloseMatrix(s_vec_cross, 0., 0.));
 
   SpatialMatrix s_vec_crossf (crossf(s_vec));
   SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose();
 
-  CHECK_EQUAL (test_crossf, s_vec_crossf);
+  REQUIRE_THAT (test_crossf, AllCloseMatrix(s_vec_crossf, 0., 0.));
 }
 
-TEST(TestSpatialVectorCrossmCrossf) {
+TEST_CASE(__FILE__"_TestSpatialVectorCrossmCrossf", "") {
   SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
   SpatialVector t_vec (9., 8., 7., 6., 5., 4.);
 
@@ -255,11 +258,10 @@ TEST(TestSpatialVectorCrossmCrossf) {
      cout << crossf_s_t << endl;
      */
 
-  CHECK_EQUAL (crossm_s_x_t, crossm_s_t);
+  REQUIRE_THAT (crossm_s_x_t, AllCloseVector(crossm_s_t, 0., 0.));
-  CHECK_EQUAL (crossf_s_x_t, crossf_s_t);
 }
 
-TEST(TestSpatialTransformApply) {
+TEST_CASE(__FILE__"_TestSpatialTransformApply", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -280,10 +282,10 @@ TEST(TestSpatialTransformApply) {
 
   // cout << (v_66_res - v_st_res).transpose() << endl;
 
-  CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC);
+  REQUIRE_THAT (v_66_res, AllCloseVector(v_st_res, TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformApplyTranspose) {
+TEST_CASE(__FILE__"_TestSpatialTransformApplyTranspose", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -304,10 +306,10 @@ TEST(TestSpatialTransformApplyTranspose) {
 
   // cout << (v_66_res - v_st_res).transpose() << endl;
 
-  CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC);
+  REQUIRE_THAT (v_66_res, AllCloseVector(v_st_res, TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformApplyAdjoint) {
+TEST_CASE(__FILE__"_TestSpatialTransformApplyAdjoint", "") {
   SpatialTransform X (
       Xrotz (0.5) *
       Xroty (0.9) *
@@ -321,10 +323,10 @@ TEST(TestSpatialTransformApplyAdjoint) {
   SpatialVector f_apply = X.applyAdjoint(f);
   SpatialVector f_matrix = X_adjoint * f;
 
-  CHECK_ARRAY_CLOSE (f_matrix.data(), f_apply.data(), 6, TEST_PREC);
+  REQUIRE_THAT (f_matrix, AllCloseVector(f_apply, TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformToMatrix) {
+TEST_CASE(__FILE__"_TestSpatialTransformToMatrix", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -338,10 +340,10 @@ TEST(TestSpatialTransformToMatrix) {
   //	SpatialMatrix X_diff = X_st.toMatrix() - X_matrix;
   //	cout << "Error: " << endl << X_diff << endl;
 
-  CHECK_ARRAY_CLOSE (X_matrix.data(), X_st.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_matrix, AllCloseMatrix(X_st.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformToMatrixAdjoint) {
+TEST_CASE(__FILE__"_TestSpatialTransformToMatrixAdjoint", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -355,10 +357,10 @@ TEST(TestSpatialTransformToMatrixAdjoint) {
   //	SpatialMatrix X_diff = X_st.toMatrixAdjoint() - spatial_adjoint(X_matrix);
   //	cout << "Error: " << endl << X_diff << endl;
 
-  CHECK_ARRAY_CLOSE (spatial_adjoint(X_matrix).data(), X_st.toMatrixAdjoint().data(), 36, TEST_PREC);
+  REQUIRE_THAT (spatial_adjoint(X_matrix), AllCloseMatrix(X_st.toMatrixAdjoint(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformToMatrixTranspose) {
+TEST_CASE(__FILE__"_TestSpatialTransformToMatrixTranspose", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -377,10 +379,10 @@ TEST(TestSpatialTransformToMatrixTranspose) {
   //	cout << "X_st: " << endl << X_st.toMatrixTranspose() << endl;
   //	cout << "X: " << endl << X_matrix_transposed() << endl;
 
-  CHECK_ARRAY_CLOSE (X_matrix_transposed.data(), X_st.toMatrixTranspose().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_matrix_transposed, AllCloseMatrix(X_st.toMatrixTranspose(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformMultiply) {
+TEST_CASE(__FILE__"_TestSpatialTransformMultiply", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -404,10 +406,10 @@ TEST(TestSpatialTransformMultiply) {
   //	SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
   //	cout << "Error: " << endl << X_diff << endl;
 
-  CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_matrix_res, AllCloseMatrix(X_st_res.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformMultiplyEqual) {
+TEST_CASE(__FILE__"_TestSpatialTransformMultiplyEqual", "") {
   Vector3d rot (1.1, 1.2, 1.3);
   Vector3d trans (1.1, 1.2, 1.3);
 
@@ -432,33 +434,33 @@ TEST(TestSpatialTransformMultiplyEqual) {
   //	SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
   //	cout << "Error: " << endl << X_diff << endl;
 
-  CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_matrix_res, AllCloseMatrix(X_st_res.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestXrotAxis) {
+TEST_CASE(__FILE__"_TestXrotAxis", "") {
   SpatialTransform X_rotX = Xrotx (M_PI * 0.15);
   SpatialTransform X_rotX_axis = Xrot (M_PI * 0.15, Vector3d (1., 0., 0.));
 
-  CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_rotX.toMatrix(), AllCloseMatrix(X_rotX_axis.toMatrix(), TEST_PREC, TEST_PREC));
 
   // all the other axes
   SpatialTransform X_rotX_90 = Xrotx (M_PI * 0.5);
   SpatialTransform X_rotX_90_axis = Xrot (M_PI * 0.5, Vector3d (1., 0., 0.));
 
-  CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_rotX_90.toMatrix(), AllCloseMatrix(X_rotX_90_axis.toMatrix(), TEST_PREC, TEST_PREC));
 
   SpatialTransform X_rotY_90 = Xroty (M_PI * 0.5);
   SpatialTransform X_rotY_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 1., 0.));
 
-  CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_rotY_90.toMatrix(), AllCloseMatrix(X_rotY_90_axis.toMatrix(), TEST_PREC, TEST_PREC));
 
   SpatialTransform X_rotZ_90 = Xrotz (M_PI * 0.5);
   SpatialTransform X_rotZ_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 0., 1.));
 
-  CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 36, TEST_PREC);
+  REQUIRE_THAT (X_rotZ_90.toMatrix(), AllCloseMatrix(X_rotZ_90_axis.toMatrix(), TEST_PREC, TEST_PREC));
 }
 
-TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) {
+TEST_CASE(__FILE__"_TestSpatialTransformApplySpatialRigidBodyInertiaAdd", "") {
   SpatialRigidBodyInertia rbi (
       1.1,
       Vector3d (1.2, 1.3, 1.4),
@@ -477,15 +479,11 @@ TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) {
   // cout << "diff = " << endl << 
   //  	rbi_added.toMatrix() - rbi_matrix_added << endl;
 
-  CHECK_ARRAY_CLOSE (
+
-      rbi_matrix_added.data(),
+  REQUIRE_THAT (rbi_matrix_added, AllCloseMatrix(rbi_added.toMatrix(), TEST_PREC, TEST_PREC));
-      rbi_added.toMatrix().data(),
-      36,
-      TEST_PREC
-      );
 }
 
-TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) {
+TEST_CASE(__FILE__"_TestSpatialTransformApplySpatialRigidBodyInertiaFull", "") {
   SpatialRigidBodyInertia rbi (
       1.1,
       Vector3d (1.2, 1.3, 1.4),
@@ -505,15 +503,10 @@ TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) {
   SpatialRigidBodyInertia rbi_transformed = X.apply (rbi);
   SpatialMatrix rbi_matrix_transformed = X.toMatrixAdjoint () * rbi.toMatrix() * X.inverse().toMatrix();
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (rbi_matrix_transformed, AllCloseMatrix(rbi_transformed.toMatrix(), TEST_PREC, TEST_PREC));
-      rbi_matrix_transformed.data(),
-      rbi_transformed.toMatrix().data(),
-      36,
-      TEST_PREC
-      );
 }
 
-TEST(TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull) {
+TEST_CASE(__FILE__"_TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull", "") {
   SpatialRigidBodyInertia rbi (
       1.1,
       Vector3d (1.2, 1.3, 1.4),
@@ -533,15 +526,10 @@ TEST(TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull) {
   SpatialRigidBodyInertia rbi_transformed = X.applyTranspose (rbi);
   SpatialMatrix rbi_matrix_transformed = X.toMatrixTranspose() * rbi.toMatrix() * X.toMatrix();
 
-  CHECK_ARRAY_CLOSE (
+  REQUIRE_THAT (rbi_matrix_transformed, AllCloseMatrix(rbi_transformed.toMatrix(), TEST_PREC, TEST_PREC));
-      rbi_matrix_transformed.data(),
-      rbi_transformed.toMatrix().data(),
-      36,
-      TEST_PREC
-      );
 }
 
-TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) {
+TEST_CASE(__FILE__"_TestSpatialRigidBodyInertiaCreateFromMatrix", "") {
   double mass = 1.1;
   Vector3d com (0., 0., 0.);
   Matrix3d inertia (
@@ -556,18 +544,18 @@ TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) {
   SpatialRigidBodyInertia rbi;
   rbi.createFromMatrix (spatial_inertia);
 
-  CHECK_EQUAL (mass, rbi.m);
+  REQUIRE_THAT (mass, IsClose(rbi.m, 0., 0.));
-  CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3);
+  REQUIRE_THAT (Vector3d(mass * com), AllCloseVector(rbi.h, 0., 0.));
   Matrix3d rbi_I_matrix (
       rbi.Ixx, rbi.Iyx, rbi.Izx,
       rbi.Iyx, rbi.Iyy, rbi.Izy,
       rbi.Izx, rbi.Izy, rbi.Izz
       );
-  CHECK_ARRAY_EQUAL (inertia.data(), rbi_I_matrix.data(), 9);
+  REQUIRE_THAT (inertia, AllCloseMatrix(rbi_I_matrix, 0., 0.));
 }
 
 #ifdef USE_SLOW_SPATIAL_ALGEBRA
-TEST(TestSpatialLinSolve) {
+TEST_CASE(__FILE__"_TestSpatialLinSolve", "") {
   SpatialVector b (1, 2, 0, 1, 1, 1);
   SpatialMatrix A (
       1., 2., 3., 0., 0., 0.,
@@ -581,6 +569,6 @@ TEST(TestSpatialLinSolve) {
   SpatialVector x = SpatialLinSolve (A, b);
   SpatialVector x_test (3.5, -6.5, 3.5, 1, 1, 1);
 
-  CHECK_ARRAY_CLOSE (x_test.data(), x.data(), 6, TEST_PREC);
+  REQUIRE_THAT (x_test, AllCloseVector(x, TEST_PREC, TEST_PREC));
 }
 #endif

3rdparty/rbdl/tests/TwolegModelTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -181,7 +181,7 @@ Vector3d heel_point (0., 0., 0.);
 Vector3d medial_point (0., 0., 0.);
 
 void init_model (Model* model) {
-  assert (model);
+  REQUIRE (model);
   
   constraint_set_right = ConstraintSet();
   constraint_set_left = ConstraintSet();
@@ -272,7 +272,7 @@ void copy_values (T *dest, const T *src, size_t count) {
   memcpy (dest, src, count * sizeof (T));
 }
 
-TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) {
+TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectFootmodel", "" ) {
   Model* model = new Model;
 
   init_model(model);
@@ -335,8 +335,8 @@ TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) {
   contact_force[0] = constraint_set_left.force[0];
   contact_force[1] = constraint_set_left.force[1];
 
-  CHECK_EQUAL (body_id, foot_left_id);
+  REQUIRE (body_id == foot_left_id);
-  CHECK_EQUAL (contact_point, heel_point);
+  REQUIRE (contact_point == heel_point);
 
   //	cout << LogOutput.str() << endl;
   contact_accel_left = CalcPointAcceleration (*model, Q, QDot, QDDot, foot_left_id, heel_point);
@@ -344,12 +344,12 @@ TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) {
   //	cout << contact_force << endl;
   //	cout << contact_accel_left << endl;
 
-  CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), contact_accel_left.data(), 3, TEST_PREC);
+  REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(contact_accel_left, TEST_PREC, TEST_PREC));
 
   delete model;
 }
 
-TEST ( TestClearContactsInertiaMatrix ) {
+TEST_CASE (__FILE__"_TestClearContactsInertiaMatrix", "") {
   Model* model = new Model;
 
   init_model(model);
@@ -398,7 +398,7 @@ TEST ( TestClearContactsInertiaMatrix ) {
   ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_right, QDDot_lag);
   ForwardDynamicsContactsKokkevis (*model, Q, QDot, Tau, constraint_set_right, QDDot_aba);
 
-  CHECK_ARRAY_CLOSE (QDDot_lag.data(), QDDot_aba.data(), QDDot.size(), TEST_PREC * QDDot_lag.norm());
+  REQUIRE_THAT (QDDot_lag, AllCloseVector(QDDot_aba, TEST_PREC * QDDot_lag.norm(), TEST_PREC * QDDot_lag.norm()));
 
   delete model;
 }

3rdparty/rbdl/tests/UtilsTests.cc

@@ -1,4 +1,4 @@
-#include <UnitTest++.h>
+#include "rbdl_tests.h"
 
 #include <iostream>
 
@@ -16,7 +16,7 @@ using namespace std;
 using namespace RigidBodyDynamics;
 using namespace RigidBodyDynamics::Math;
 
-TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) {
+TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestKineticEnergy", "") {
   VectorNd q = VectorNd::Zero(model->q_size);
   VectorNd qdot = VectorNd::Zero(model->q_size);
 
@@ -31,10 +31,10 @@ TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) {
   double kinetic_energy_ref = 0.5 * qdot.transpose() * H * qdot;
   double kinetic_energy = Utils::CalcKineticEnergy (*model, q, qdot);
 
-  CHECK_EQUAL (kinetic_energy_ref, kinetic_energy);
+  REQUIRE (kinetic_energy_ref == kinetic_energy);
 }
 
-TEST(TestPotentialEnergy) {
+TEST_CASE (__FILE__"_TestPotentialEnergy", "") {
   Model model;
   Matrix3d inertia = Matrix3d::Zero(3,3);
   Body body (0.5, Vector3d (0., 0., 0.), inertia);
@@ -48,14 +48,14 @@ TEST(TestPotentialEnergy) {
 
   VectorNd q = VectorNd::Zero(model.q_size);
   double potential_energy_zero = Utils::CalcPotentialEnergy (model, q);
-  CHECK_EQUAL (0., potential_energy_zero);
+  REQUIRE (0. == potential_energy_zero);
 
   q[1] = 1.;
   double potential_energy_lifted = Utils::CalcPotentialEnergy (model, q);
-  CHECK_EQUAL (4.905, potential_energy_lifted);
+  REQUIRE (4.905 == potential_energy_lifted);
 }
 
-TEST(TestCOMSimple) {
+TEST_CASE (__FILE__"_TestCOMSimple", "") {
   Model model;
   Matrix3d inertia = Matrix3d::Zero(3,3);
   Body body (123., Vector3d (0., 0., 0.), inertia);
@@ -75,22 +75,22 @@ TEST(TestCOMSimple) {
   Vector3d com_velocity;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity);
 
-  CHECK_EQUAL (123., mass);
+  REQUIRE (123. == mass);
-  CHECK_EQUAL (Vector3d (0., 0., 0.), com);
+  REQUIRE (Vector3d (0., 0., 0.) == com);
-  CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity);
+  REQUIRE (Vector3d (0., 0., 0.) == com_velocity);
 
   q[1] = 1.;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity);
-  CHECK_EQUAL (Vector3d (0., 1., 0.), com);
+  REQUIRE (Vector3d (0., 1., 0.) == com);
-  CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity);
+  REQUIRE (Vector3d (0., 0., 0.) == com_velocity);
 
   qdot[1] = 1.;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity);
-  CHECK_EQUAL (Vector3d (0., 1., 0.), com);
+  REQUIRE (Vector3d (0., 1., 0.) == com);
-  CHECK_EQUAL (Vector3d (0., 1., 0.), com_velocity);
+  REQUIRE (Vector3d (0., 1., 0.) == com_velocity);
 }
 
-TEST(TestAngularMomentumSimple) {
+TEST_CASE (__FILE__"_TestAngularMomentumSimple", "") {
   Model model;
   Matrix3d inertia = Matrix3d::Zero(3,3);
   inertia(0,0) = 1.1;
@@ -115,25 +115,25 @@ TEST(TestAngularMomentumSimple) {
 
   qdot << 1., 0., 0.;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
-  CHECK_EQUAL (Vector3d (1.1, 0., 0.), angular_momentum);
+  REQUIRE (Vector3d (1.1, 0., 0.) == angular_momentum);
 
   qdot << 0., 1., 0.;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
-  CHECK_EQUAL (Vector3d (0., 2.2, 0.), angular_momentum);
+  REQUIRE (Vector3d (0., 2.2, 0.) == angular_momentum);
 
   qdot << 0., 0., 1.;
   Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
-  CHECK_EQUAL (Vector3d (0., 0., 3.3), angular_momentum);
+  REQUIRE (Vector3d (0., 0., 3.3) == angular_momentum);
 }
 
-TEST_FIXTURE (TwoArms12DoF, TestAngularMomentumSimple) {
+TEST_CASE_METHOD (TwoArms12DoF, __FILE__"_TestAngularMomentumSimple2", "") {
   double mass;
   Vector3d com;
   Vector3d angular_momentum;
 
   Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
 
-  CHECK_EQUAL (Vector3d (0., 0., 0.), angular_momentum);
+  REQUIRE (Vector3d (0., 0., 0.) == angular_momentum);
 
   qdot[0] = 1.;
   qdot[1] = 2.;
@@ -142,7 +142,7 @@ TEST_FIXTURE (TwoArms12DoF, TestAngularMomentumSimple) {
   Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
 
   // only a rough guess from test calculation
-  CHECK_ARRAY_CLOSE (Vector3d (3.3, 2.54, 1.5).data(), angular_momentum.data(), 3, 1.0e-1);
+  REQUIRE_THAT (Vector3d (3.3, 2.54, 1.5), AllCloseVector(angular_momentum, 1.0e-1, 1.0e-1));
 
   qdot[3] = -qdot[0];
   qdot[4] = -qdot[1];
@@ -151,9 +151,9 @@ TEST_FIXTURE (TwoArms12DoF, TestAngularMomentumSimple) {
   ClearLogOutput();
   Utils::CalcCenterOfMass (*model, q, qdot, NULL, mass, com, NULL, NULL, &angular_momentum);
 
-  CHECK (angular_momentum[0] == 0);
+  REQUIRE (angular_momentum[0] == 0);
-  CHECK (angular_momentum[1] < 0);
+  REQUIRE (angular_momentum[1] < 0);
-  CHECK (angular_momentum[2] == 0.);
+  REQUIRE (angular_momentum[2] == 0.);
 }
 
 template <typename T>
@@ -183,36 +183,24 @@ void TestCoMComputation (
     NULL, NULL
   );
 
-  CHECK_CLOSE (mass_expected, mass_actual, 1e-7);
+  REQUIRE_THAT (mass_expected, IsClose(mass_actual, 1e-7, 1e-7));
 
   return;
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestCoMComputationLinearInvertedPendulumModel", "") {
-  LinearInvertedPendulumModel,
-  TestCoMComputationLinearInvertedPendulumModel
-) {
   TestCoMComputation (*this);
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (FixedJoint2DoF, __FILE__"_TestCoMComputationFixedJoint2DoF", "") {
-  FixedJoint2DoF,
-  TestCoMComputationFixedJoint2DoF
-) {
   TestCoMComputation (*this);
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_TestCoMComputationFixedBase6DoF12DoFFloatingBase", "") {
-  FixedBase6DoF12DoFFloatingBase,
-  TestCoMComputationFixedBase6DoF12DoFFloatingBase
-) {
   TestCoMComputation (*this);
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (Human36, __FILE__"_TestCoMComputationHuman36", "") {
-  Human36,
-  TestCoMComputationHuman36
-) {
   TestCoMComputation (*this);
 }
 
@@ -261,30 +249,21 @@ void TestCoMAccelerationUsingFD (
   ch_ang_mom_fd = (ch_ang_mom_fd - ang_mom) / EPS;
 
   // check CoM acceleration
-  CHECK_ARRAY_CLOSE (com_acc_nom.data(), com_acc_fd.data(), 3, TOL);
+  REQUIRE_THAT (com_acc_nom, AllCloseVector(com_acc_fd, TOL, TOL));
-  CHECK_ARRAY_CLOSE (ch_ang_mom_nom.data(), ch_ang_mom_fd.data(), 3, TOL);
+  REQUIRE_THAT (ch_ang_mom_nom, AllCloseVector(ch_ang_mom_fd, TOL, TOL));
 
   return;
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestCoMAccelerationUsingFDLinearInvertedPendulumModel", "") {
-  LinearInvertedPendulumModel,
-  TestCoMAccelerationUsingFDLinearInvertedPendulumModel
-) {
   TestCoMAccelerationUsingFD (*this, 1e-8);
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (FixedJoint2DoF, __FILE__"_TestCoMAccelerationUsingFDFixedJoint2DoF", "") {
-  FixedJoint2DoF,
-  TestCoMAccelerationUsingFDFixedJoint2DoF
-) {
   TestCoMAccelerationUsingFD (*this, 1e-7);
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_TestCoMAccelerationUsingFDFixedBase6DoF12DoFFloatingBase", "") {
-  FixedBase6DoF12DoFFloatingBase,
-  TestCoMAccelerationUsingFDFixedBase6DoF12DoFFloatingBase
-) {
   TestCoMAccelerationUsingFD (*this, 1e-6);
 }
 
@@ -321,15 +300,12 @@ void TestZMPComputationForNotMovingSystem(
   com = com - distance * obj.contact_normal;
 
   // check ZMP against CoM
-  CHECK_ARRAY_CLOSE (com.data(), zmp.data(), 3, TOL);
+  REQUIRE_THAT (com, AllCloseVector(zmp, TOL, TOL));
 
   return;
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestZMPComputationForNotMovingSystemLinearInvertedPendulumModel", "") {
-  LinearInvertedPendulumModel,
-  TestZMPComputationForNotMovingSystemLinearInvertedPendulumModel
-) {
   TestZMPComputationForNotMovingSystem (*this, 1e-8);
 }
 
@@ -365,14 +341,11 @@ void TestZMPComputationAgainstTableCartModel(
   );
 
   // check ZMP against CoM
-  CHECK_ARRAY_CLOSE (com.data(), zmp.data(), 3, TOL);
+  REQUIRE_THAT (com, AllCloseVector(zmp, TOL, TOL));
 
   return;
 }
 
-TEST_FIXTURE(
+TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestZMPComputationAgainstTableCartModelLinearInvertedPendulumModel", "") {
-  LinearInvertedPendulumModel,
-  TestZMPComputationAgainstTableCartModelLinearInvertedPendulumModel
-) {
   TestZMPComputationAgainstTableCartModel (*this, 1e-8);
 }

3rdparty/rbdl/tests/main.cc

@@ -1,4 +1,6 @@
-#include <UnitTest++.h>
+#define CATCH_CONFIG_RUNNER
+#include "catch.hpp"
+
 #include <iostream>
 #include <string>
 
@@ -15,5 +17,5 @@ int main (int argc, char *argv[])
       rbdl_print_version();
   }
 
-  return UnitTest::RunAllTests ();
+  return Catch::Session().run(argc, argv);
 }

CMakeLists.txt

@@ -9,7 +9,7 @@ project (rbdlsim
 find_package(Threads)
 
 # RBDL
-set(RBDL_USE_SIMPLE_MATH On)
+set(RBDL_USE_SIMPLE_MATH TRUE)
 add_subdirectory (3rdparty/rbdl)
 
 # libccd