diff --git a/3rdparty/rbdl/CMakeLists.txt b/3rdparty/rbdl/CMakeLists.txt index 567906d..7c4dd30 100644 --- a/3rdparty/rbdl/CMakeLists.txt +++ b/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 ) diff --git a/3rdparty/rbdl/README.md b/3rdparty/rbdl/README.md index 27f993c..b3b6077 100644 --- a/3rdparty/rbdl/README.md +++ b/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 =============== diff --git a/3rdparty/rbdl/addons/luamodel/luamodel.cc b/3rdparty/rbdl/addons/luamodel/luamodel.cc index 238b033..beb348d 100644 --- a/3rdparty/rbdl/addons/luamodel/luamodel.cc +++ b/3rdparty/rbdl/addons/luamodel/luamodel.cc @@ -13,6 +13,8 @@ extern "C" #include } + + using namespace std; using namespace RigidBodyDynamics; using namespace RigidBodyDynamics::Math; diff --git a/3rdparty/rbdl/include/rbdl/Dynamics.h b/3rdparty/rbdl/include/rbdl/Dynamics.h index e039bdb..eaa41a1 100644 --- a/3rdparty/rbdl/include/rbdl/Dynamics.h +++ b/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 @@ -144,7 +144,7 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian ( Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR, std::vector *f_ext = NULL, Math::MatrixNd *H = NULL, - Math::VectorNd *C = NULL + Math::VectorNd *C = NULL ); /** \brief Computes the effect of multiplying the inverse of the joint @@ -158,7 +158,7 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian ( * \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost) * * This function uses a reduced version of the Articulated %Body Algorithm - * to compute: + * to compute: * * \f$ \ddot{q} = M(q)^{-1} \tau\f$ * diff --git a/3rdparty/rbdl/include/rbdl/Quaternion.h b/3rdparty/rbdl/include/rbdl/Quaternion.h index 324a554..ca9d3f5 100644 --- a/3rdparty/rbdl/include/rbdl/Quaternion.h +++ b/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()); - - // division by 0.f is unhealthy! - assert (s != 0.); - - double angle = acos (dot(quat) / s); - if (angle == 0. || std::isnan(angle)) { + double cos_half_theta = this->dot(quat); + if (fabs(cos_half_theta >= 1.0)) { return *this; } - assert(!std::isnan(angle)); - double d = 1. / std::sin (angle); - double p0 = std::sin ((1. - alpha) * angle); - double p1 = std::sin (alpha * angle); + double half_theta = acos(cos_half_theta); + double sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta); - if (dot (quat) < 0.) { - return Quaternion( ((*this) * p0 - quat * p1) * d); + if (fabs(sin_half_theta) < 0.00001) { + 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,18 +108,45 @@ 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; - 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); + 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) { return Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0]) * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1]) - * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]); + * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]); } static Quaternion fromYXZAngles (const Vector3d &yxz_angles) { @@ -122,7 +156,7 @@ class Quaternion : public Vector4d { } static Quaternion fromXYZAngles (const Vector3d &xyz_angles) { - return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2]) + return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2]) * Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), xyz_angles[1]) * Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), xyz_angles[0]); } diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h index 2e867e1..06cbcd2 100644 --- a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h +++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h @@ -819,19 +819,16 @@ struct Storage { inline size_t cols() const { return NumCols; } -#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 - + void resize(int UNUSED(num_rows), int UNUSED(num_cols)) { + // Resizing of fixed size matrices not allowed + #ifndef NDEBUG + 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; + } + #endif + assert (num_rows == NumRows && num_cols == NumCols); } inline ScalarType& coeff(int row_index, int col_index) { @@ -1555,6 +1552,7 @@ private: typedef Matrix ColumnVector; bool mIsFactorized; + Matrix 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()); - - // division by 0.f is unhealthy! - assert (s != 0.f); - - float angle = acos (dot(quat) / s); - if (angle == 0.f || std::isnan(angle)) { + double cos_half_theta = this->dot(quat); + if (fabs(cos_half_theta >= 1.0)) { return *this; } - assert(!std::isnan(angle)); - float d = 1.f / sinf (angle); - float p0 = sinf ((1.f - alpha) * angle); - float p1 = sinf (alpha * angle); + double half_theta = acos(cos_half_theta); + double sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta); - if (dot (quat) < 0.f) { - return Quaternion( ((*this) * p0 - quat * p1) * d); + if (fabs(sin_half_theta) < 0.00001) { + 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; - return Quaternion ( - (mat(2,1) - mat(1,2)) / (w * 4.f), - (mat(0,2) - mat(2,0)) / (w * 4.f), - (mat(1,0) - mat(0,1)) / (w * 4.f), - w); + 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 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) { diff --git a/3rdparty/rbdl/src/Joint.cc b/3rdparty/rbdl/src/Joint.cc index 894d155..876f211 100644 --- a/3rdparty/rbdl/src/Joint.cc +++ b/3rdparty/rbdl/src/Joint.cc @@ -14,6 +14,21 @@ #include "rbdl/Model.h" #include "rbdl/Joint.h" +#ifdef __APPLE__ +#include +inline void sincos(double x, double * sinp, double * cosp) { + return __sincos(x, sinp, cosp); +} +#endif + +#if _MSC_VER || defined(__QNX__) +#include +inline void sincos(double x, double * sinp, double * cosp) { + *sinp = sin(x); + *cosp = cos(x); +} +#endif + namespace RigidBodyDynamics { using namespace Math; @@ -92,21 +107,21 @@ RBDL_DLLAPI void jcalc ( jcalc_X_lambda_S(model, joint_id, q); double Jqd = qdot[model.mJoints[joint_id].q_index]; model.v_J[joint_id] = model.S[joint_id] * Jqd; - + Vector3d St = model.S[joint_id].block(0,0,3,1); Vector3d c = X_J.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); c = St.cross(c); - c *= -Jqd * Jqd; + c *= -Jqd * Jqd; model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]); model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; } else if (model.mJoints[joint_id].mDoFCount == 1 && model.mJoints[joint_id].mJointType != JointTypeCustom) { SpatialTransform X_J = jcalc_XJ (model, joint_id, q); - model.v_J[joint_id] = + model.v_J[joint_id] = model.S[joint_id] * qdot[model.mJoints[joint_id].q_index]; model.X_lambda[joint_id] = X_J * model.X_T[joint_id]; } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { - SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(), + SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(), Vector3d (0., 0., 0.)); model.multdof3_S[joint_id](0,0) = 1.; @@ -153,7 +168,7 @@ RBDL_DLLAPI void jcalc ( double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; - model.v_J[joint_id] = + model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); model.c_J[joint_id].set( @@ -194,7 +209,7 @@ RBDL_DLLAPI void jcalc ( double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; - model.v_J[joint_id] = + model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); model.c_J[joint_id].set( @@ -235,7 +250,7 @@ RBDL_DLLAPI void jcalc ( double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; - model.v_J[joint_id] = + model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); model.c_J[joint_id].set( @@ -279,7 +294,7 @@ RBDL_DLLAPI void jcalc ( double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; - model.v_J[joint_id] = + model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); model.c_J[joint_id].set( @@ -301,7 +316,7 @@ RBDL_DLLAPI void jcalc ( double qdot1 = qdot[model.mJoints[joint_id].q_index + 1]; double qdot2 = qdot[model.mJoints[joint_id].q_index + 2]; - model.v_J[joint_id] = + model.v_J[joint_id] = model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2); model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.); @@ -309,7 +324,7 @@ RBDL_DLLAPI void jcalc ( model.X_lambda[joint_id].r = model.X_T[joint_id].r + model.X_T[joint_id].E.transpose() * Vector3d (q0, q1, q2); } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { const Joint &joint = model.mJoints[joint_id]; - CustomJoint *custom_joint = + CustomJoint *custom_joint = model.mCustomJoints[joint.custom_joint_index]; custom_joint->jcalc (model, joint_id, q, qdot); } else { @@ -337,9 +352,9 @@ RBDL_DLLAPI Math::SpatialTransform jcalc_XJ ( return Xtrans ( Vector3d ( model.mJoints[joint_id].mJointAxes[0][3] * q[model.mJoints[joint_id].q_index], - model.mJoints[joint_id].mJointAxes[0][4] + model.mJoints[joint_id].mJointAxes[0][4] * q[model.mJoints[joint_id].q_index], - model.mJoints[joint_id].mJointAxes[0][5] + model.mJoints[joint_id].mJointAxes[0][5] * q[model.mJoints[joint_id].q_index] ) ); @@ -353,9 +368,9 @@ RBDL_DLLAPI Math::SpatialTransform jcalc_XJ ( SpatialTransform trans = Xtrans ( Vector3d ( model.mJoints[joint_id].mJointAxes[0][3] * q[model.mJoints[joint_id].q_index], - model.mJoints[joint_id].mJointAxes[0][4] + model.mJoints[joint_id].mJointAxes[0][4] * q[model.mJoints[joint_id].q_index], - model.mJoints[joint_id].mJointAxes[0][5] + model.mJoints[joint_id].mJointAxes[0][5] * q[model.mJoints[joint_id].q_index] ) ); @@ -440,14 +455,14 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.X_lambda[joint_id] = XJ * model.X_T[joint_id]; // Set the joint axis Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1); - + model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0], model.mJoints[joint_id].mJointAxes[0][1], model.mJoints[joint_id].mJointAxes[0][2], trans[0], trans[1], trans[2]); } else if (model.mJoints[joint_id].mDoFCount == 1 && model.mJoints[joint_id].mJointType != JointTypeCustom){ - model.X_lambda[joint_id] = + model.X_lambda[joint_id] = jcalc_XJ (model, joint_id, q) * model.X_T[joint_id]; model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0]; } else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) { @@ -471,7 +486,7 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( double s2 = sin (q2); double c2 = cos (q2); - model.X_lambda[joint_id] = SpatialTransform ( + model.X_lambda[joint_id] = SpatialTransform ( Matrix3d( c0 * c1, s0 * c1, -s1, c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2, @@ -590,7 +605,7 @@ RBDL_DLLAPI void jcalc_X_lambda_S ( model.multdof3_S[joint_id](5,2) = 1.; } else if (model.mJoints[joint_id].mJointType == JointTypeCustom) { const Joint &joint = model.mJoints[joint_id]; - CustomJoint *custom_joint + CustomJoint *custom_joint = model.mCustomJoints[joint.custom_joint_index]; custom_joint->jcalc_X_lambda_S (model, joint_id, q); diff --git a/3rdparty/rbdl/tests/BodyTests.cc b/3rdparty/rbdl/tests/BodyTests.cc index 476fe2c..4266cd2 100644 --- a/3rdparty/rbdl/tests/BodyTests.cc +++ b/3rdparty/rbdl/tests/BodyTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); + REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_rbi.toMatrix(), TEST_PREC, 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); - CHECK_ARRAY_CLOSE (inertia_C.data(), body.mInertia.data(), 9, TEST_PREC); + REQUIRE_THAT (reference_inertia, AllCloseMatrix(body_rbi.toMatrix(), TEST_PREC, 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); - CHECK_ARRAY_CLOSE (body.mCenterOfMass.data(), joined_body.mCenterOfMass.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (body_rbi.toMatrix().data(), joined_body_rbi.toMatrix().data(), 36, TEST_PREC); + REQUIRE (1.1 == body.mMass); + REQUIRE_THAT (body.mCenterOfMass, AllCloseMatrix(joined_body.mCenterOfMass, TEST_PREC, 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); - CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); - CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); + REQUIRE (2.2 == body_joined.mMass); + REQUIRE_THAT (Vector3d (0., 1.3, 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.)); + 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); - CHECK_ARRAY_EQUAL (Vector3d (0., 1.3, 0.).data(), body_joined.mCenterOfMass.data(), 3); - CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); - - + REQUIRE (2.2 == body_joined.mMass); + REQUIRE_THAT (Vector3d (0., 1.3, 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.)); + 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); - CHECK_ARRAY_EQUAL (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3); - CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); + REQUIRE (2.2 == body_joined.mMass); + REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(body_joined.mCenterOfMass, 0., 0.)); + 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); - CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), body_joined.mCenterOfMass.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (reference_inertia.data(), body_joined_rbi.toMatrix().data(), 36, TEST_PREC); + REQUIRE (2.2 == body_joined.mMass); + REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(body_joined.mCenterOfMass, TEST_PREC, 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 ( - fv_matrix.data(), - fv_rbi.data(), - 6, - TEST_PREC - ); + REQUIRE_THAT (fv_matrix, AllCloseMatrix(fv_rbi, TEST_PREC, 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 ( - spatial_inertia.data(), - rbi.toMatrix().data(), - 36, - TEST_PREC - ); + REQUIRE_THAT (spatial_inertia, AllCloseMatrix(rbi.toMatrix(), TEST_PREC, 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 ( - rbi.toMatrix().data(), - rbi_from_matrix.toMatrix().data(), - 36, - TEST_PREC - ); + REQUIRE_THAT (rbi.toMatrix(), AllCloseMatrix(rbi_from_matrix.toMatrix(), TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/CMakeLists.txt b/3rdparty/rbdl/tests/CMakeLists.txt index b6d681e..c9c8291 100644 --- a/3rdparty/rbdl/tests/CMakeLists.txt +++ b/3rdparty/rbdl/tests/CMakeLists.txt @@ -1,39 +1,31 @@ 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 - KinematicsTests.cc - BodyTests.cc - ModelTests.cc - FloatingBaseTests.cc - CalcVelocitiesTests.cc - CalcAccelerationsTests.cc - DynamicsTests.cc - InverseDynamicsTests.cc - CompositeRigidBodyTests.cc - ImpulsesTests.cc - TwolegModelTests.cc - ContactsTests.cc - UtilsTests.cc - SparseFactorizationTests.cc - CustomJointSingleBodyTests.cc - CustomJointMultiBodyTests.cc - CustomConstraintsTests.cc - InverseKinematicsTests.cc - LoopConstraintsTests.cc - ScrewJointTests.cc - ForwardDynamicsConstraintsExternalForces.cc - ) + MathTests.cc + SpatialAlgebraTests.cc + MultiDofTests.cc + KinematicsTests.cc + BodyTests.cc + ModelTests.cc + FloatingBaseTests.cc + CalcVelocitiesTests.cc + CalcAccelerationsTests.cc + DynamicsTests.cc + InverseDynamicsTests.cc + CompositeRigidBodyTests.cc + ImpulsesTests.cc + TwolegModelTests.cc + ContactsTests.cc + UtilsTests.cc + SparseFactorizationTests.cc + CustomJointSingleBodyTests.cc + CustomJointMultiBodyTests.cc + CustomConstraintsTests.cc + InverseKinematicsTests.cc + LoopConstraintsTests.cc + ScrewJointTests.cc + ForwardDynamicsConstraintsExternalForces.cc +) INCLUDE_DIRECTORIES ( ../src/ ) @@ -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,15 +47,16 @@ 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} - ) + ${RBDL_LIBRARY} + ) OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF) diff --git a/3rdparty/rbdl/tests/CalcAccelerationsTests.cc b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc index 644cec0..a06a7c5 100644 --- a/3rdparty/rbdl/tests/CalcAccelerationsTests.cc +++ b/3rdparty/rbdl/tests/CalcAccelerationsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_CLOSE(1., point_acceleration[1], TEST_PREC); - CHECK_CLOSE(0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (0., 1., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); ClearLogOutput(); @@ -67,12 +61,10 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointRotation) { // cout << LogOutput.str() << endl; - CHECK_CLOSE( 1., point_acceleration[0], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE(-1., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (0., -1., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 1., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (0., 1., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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); - CHECK_CLOSE( 0., point_acceleration[1], TEST_PREC); - CHECK_CLOSE( 0., point_acceleration[2], TEST_PREC); + REQUIRE_THAT(Vector3d (-1., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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(), - point_acceleration.data(), - 3, - TEST_PREC); + REQUIRE_THAT (point_acceleration_reference, AllCloseVector(point_acceleration, TEST_PREC, 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(), - point_acceleration.data(), - 3, - TEST_PREC); + REQUIRE_THAT (point_acceleration_reference, AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/CalcVelocitiesTests.cc b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc index 059a16c..c68c16a 100644 --- a/3rdparty/rbdl/tests/CalcVelocitiesTests.cc +++ b/3rdparty/rbdl/tests/CalcVelocitiesTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_CLOSE(1., point_velocity[1], TEST_PREC); - CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); - CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(-1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); - CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(0., 0., -1.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE( 2., point_velocity[1], TEST_PREC); - CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(0., 2., -1.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE( 0., point_velocity[1], TEST_PREC); - CHECK_CLOSE(-1., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(-2., 0., -1.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE( 1., point_velocity[1], TEST_PREC); - CHECK_CLOSE( 0., point_velocity[2], TEST_PREC); + REQUIRE_THAT(Vector3d(0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (Vector3d (0., 2., 0.).data(), point1_velocity.data(), 3, TEST_PREC); + REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(point0_velocity, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (Vector3d (-2., 0., 0.).data(), point1_velocity.data(), 3, TEST_PREC); + REQUIRE_THAT (Vector3d (-1., 0., 0.), AllCloseVector(point0_velocity, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d (-2., 0., 0.), AllCloseVector(point1_velocity, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc index 6ce896c..d054010 100644 --- a/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc +++ b/3rdparty/rbdl/tests/CompositeRigidBodyTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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)); } diff --git a/3rdparty/rbdl/tests/ContactsTests.cc b/3rdparty/rbdl/tests/ContactsTests.cc index 1ef580a..cc51811 100644 --- a/3rdparty/rbdl/tests/ContactsTests.cc +++ b/3rdparty/rbdl/tests/ContactsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -100,10 +100,10 @@ struct FixedBase6DoF9DoF { ConstraintSet constraint_set; }; -// -// ForwardDynamicsConstraintsDirect -// -TEST ( TestForwardDynamicsConstraintsDirectSimple ) { +// +// ForwardDynamicsConstraintsDirect +// +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 ( - Vector3d (0., 0., 0.).data(), - point_acceleration.data(), - 3, - TEST_PREC - ); + REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, 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 ( - Vector3d (0., 0., 0.).data(), - point_acceleration.data(), - 3, - TEST_PREC - ); + REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC)); // cout << "LagrangianSimple Logoutput Start" << endl; // cout << LogOutput.str() << endl; @@ -229,8 +219,8 @@ 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); - CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); - CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), 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)); + REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, 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); - CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC); - CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), 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_opt), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts_opt, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts_opt, TEST_PREC, TEST_PREC)); } // @@ -301,8 +291,8 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) { // - ForwardDynamcsContactsOpt // 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); - - CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC); - CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), 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)); + REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC)); - CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC)); - CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC)); - CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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 ( - constraint_set_lagrangian.force.data(), - constraint_set.force.data(), - constraint_set.size(), TEST_PREC - ); - - CHECK_CLOSE (0., point_accel_c[0], TEST_PREC); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC)); + + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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); - CHECK_CLOSE (0., point_accel_c[1], TEST_PREC); - CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC); + REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, 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.); - CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.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.)); + 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); - CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_right_velocity.data(), 3, TEST_PREC); + REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_left_velocity, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_right_velocity, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/CustomConstraintsTests.cc b/3rdparty/rbdl/tests/CustomConstraintsTests.cc index 7824ddb..ee35bfd 100644 --- a/3rdparty/rbdl/tests/CustomConstraintsTests.cc +++ b/3rdparty/rbdl/tests/CustomConstraintsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include "rbdl/rbdl.h" #include "rbdl/Constraints.h" #include @@ -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); - CHECK(errd.norm() >= qDotError); + REQUIRE (err.norm() >= qError); + 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 +#include "rbdl_tests.h" #include @@ -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 ( - reference_model.at(idx).X_base[ - 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); + REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, + AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC) + ); - CHECK_ARRAY_CLOSE ( - reference_model.at(idx).v[ - 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); + REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, + AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC) + ); - CHECK_ARRAY_CLOSE ( - reference_model.at(idx).a[ - 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); + REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)], + AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC, TEST_PREC) + ); + + REQUIRE_THAT (reference_model.at(idx).a[reference_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) + ); } } -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 ( - reference_model.at(idx).X_base[ - 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); + REQUIRE_THAT (reference_model.at(idx).X_base[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, + AllCloseMatrix(custom_model.at(idx).X_base[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)].E, TEST_PREC, TEST_PREC) + ); //velocity @@ -602,15 +580,9 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) { &q.at(idx), &qdot.at(idx), NULL); - CHECK_ARRAY_CLOSE ( - reference_model.at(idx).v[ - 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); + REQUIRE_THAT (reference_model.at(idx).v[reference_body_id.at(idx).at(NUMBER_OF_BODIES-1)], + AllCloseVector(custom_model.at(idx).v[custom_body_id.at(idx).at(NUMBER_OF_BODIES-1)], TEST_PREC,TEST_PREC) + ); //All @@ -624,21 +596,13 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, UpdateKinematicsCustom) { &qdot.at(idx), &qddot.at(idx)); - CHECK_ARRAY_CLOSE ( - reference_model.at(idx).a[ - 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); - } - - + REQUIRE_THAT (reference_model.at(idx).a[reference_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) + ); + } } -TEST_FIXTURE (CustomJointMultiBodyFixture, Jacobians) { +TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_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){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - Gref(i,j), - Gcus(i,j), - TEST_PREC); - } - } + REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, 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){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - Gref(i,j), - Gcus(i,j), - TEST_PREC); - } - } + REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, 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){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - GrefPt(i,j), - GcusPt(i,j), - TEST_PREC); - } - } + REQUIRE_THAT (GrefPt, AllCloseMatrix(GcusPt, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointMultiBodyFixture, InverseDynamics) { - +TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_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 ( - tauRef.data(), - tauCus.data(), - tauRef.rows(), - TEST_PREC); + REQUIRE_THAT (tauRef, AllCloseVector(tauCus, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm) { - +TEST_CASE_METHOD (CustomJointMultiBodyFixture, __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++) { @@ -844,15 +779,11 @@ TEST_FIXTURE (CustomJointMultiBodyFixture, CompositeRigidBodyAlgorithm) { c_cus * -1. + tau.at(idx), qddot_crba_cus); - CHECK_ARRAY_CLOSE(qddot_crba_ref.data(), - qddot_crba_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT(qddot_crba_ref, AllCloseVector(qddot_crba_cus, TEST_PREC, 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 ( - qddotRef.data(), - qddotCus.data(), - dof, - TEST_PREC); + REQUIRE_THAT (qddotRef, AllCloseVector(qddotCus, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointMultiBodyFixture, CalcMInvTimestau) { - +TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_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(), - qddot_minv_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT(qddot_minv_ref, AllCloseVector(qddot_minv_cus, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointMultiBodyFixture, ForwardDynamicsContactsKokkevis){ - +TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_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(), - qdot_plus_cus.data(), - dof, - TEST_PREC); - - CHECK_ARRAY_CLOSE (qddot_ref.data(), - qddot_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT (qdot_plus_ref, AllCloseVector(qdot_plus_cus, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qddot_ref, AllCloseVector(qddot_cus, TEST_PREC, TEST_PREC)); } } - } // diff --git a/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc index 5962181..46ca8ff 100644 --- a/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc +++ b/3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc @@ -4,7 +4,7 @@ */ -#include +#include "rbdl_tests.h" #include @@ -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 ( - 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); - - 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); + 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)); + 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)); + 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)); } } -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); - - 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); - + 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)); //velocity UpdateKinematicsCustom (reference_model.at(idx), @@ -445,12 +426,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) { &qdot.at(idx), NULL); - 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); - + 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)); //All UpdateKinematicsCustom (reference_model.at(idx), @@ -463,18 +439,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) { &qdot.at(idx), &qddot.at(idx)); - 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); - } - - + 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)); + } } -TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) { - +TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_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,15 +480,8 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) { custom_body_id.at(idx), Gcus); - for(int i=0; i<6;++i){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - Gref(i,j), - Gcus(i,j), - TEST_PREC); - } - } - + REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, 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){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - Gref(i,j), - Gcus(i,j), - TEST_PREC); - } - } - - + REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, 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){ - for(unsigned int j = 0; j < dof; ++j){ - CHECK_CLOSE ( - GrefPt(i,j), - GcusPt(i,j), - TEST_PREC); - } - } + REQUIRE_THAT (GrefPt, AllCloseMatrix(GcusPt, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) { - +TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_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 ( - tauRef.data(), - tauCus.data(), - tauRef.rows(), - TEST_PREC); + REQUIRE_THAT (tauRef, AllCloseVector(tauCus, TEST_PREC, 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(), - qddot_crba_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT(qddot_crba_ref, AllCloseVector(qddot_crba_cus, TEST_PREC, 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(), - qddotCus.data(), - dof, - TEST_PREC); + REQUIRE_THAT ( qddotRef, AllCloseVector(qddotCus, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) { +TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_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(), - qddot_minv_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT(qddot_minv_ref, AllCloseVector(qddot_minv_cus, TEST_PREC, TEST_PREC)); } - } -TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){ - +TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_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(), - qdot_plus_cus.data(), - dof, - TEST_PREC); - - CHECK_ARRAY_CLOSE (qddot_ref.data(), - qddot_cus.data(), - dof, - TEST_PREC); + REQUIRE_THAT (qdot_plus_ref, AllCloseVector(qdot_plus_cus, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qddot_ref, AllCloseVector(qddot_cus, TEST_PREC, TEST_PREC)); } } - } - - diff --git a/3rdparty/rbdl/tests/CustomJointTests.cc b/3rdparty/rbdl/tests/CustomJointTests.cc index 9f19344..303a19e 100644 --- a/3rdparty/rbdl/tests/CustomJointTests.cc +++ b/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!"); } }; diff --git a/3rdparty/rbdl/tests/DynamicsTests.cc b/3rdparty/rbdl/tests/DynamicsTests.cc index 5914272..908b3b4 100644 --- a/3rdparty/rbdl/tests/DynamicsTests.cc +++ b/3rdparty/rbdl/tests/DynamicsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include #include @@ -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); - CHECK_CLOSE ( 3.92400000000000E+00, QDDot[1], TEST_PREC); + REQUIRE_THAT (-5.88600000000000E+00, IsClose(QDDot[0], TEST_PREC, 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); - CHECK_CLOSE ( 3.77307692307692E+00, QDDot[1], TEST_PREC); - CHECK_CLOSE ( 1.50923076923077E+00, QDDot[2], TEST_PREC); + REQUIRE_THAT (Vector3d(-6.03692307692308E+00, 3.77307692307692E+00, 1.50923076923077E+00), AllCloseVector(QDDot, TEST_PREC, 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); - CHECK_CLOSE ( 0.00000000000000E+00, QDDot[1], TEST_PREC); + REQUIRE_THAT (-3.92400000000000E+00, IsClose(QDDot[0], TEST_PREC, 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); - CHECK_CLOSE (-5.34396887159533E-01, QDDot[1], TEST_PREC); - CHECK_CLOSE ( 4.10340466926070E+00, QDDot[2], TEST_PREC); - CHECK_CLOSE ( 2.67198443579767E-01, QDDot[3], TEST_PREC); - CHECK_CLOSE ( 5.30579766536965E+00, QDDot[4], TEST_PREC); + VectorNd target = VectorNd::Zero (5); + target[0] = -1.60319066147860E+00; + target[1] = -5.34396887159533E-01; + target[2] = 4.10340466926070E+00; + 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()); - CHECK_ARRAY_CLOSE (QDDot_aba.data(), QDDot_lagrangian.data(), QDDot_aba.size(), TEST_PREC); + REQUIRE (QDDot_aba.size() == QDDot_lagrangian.size()); + 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(i + 1) * 0.1; QDot[i] = static_cast(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(RAND_MAX); Tau[i] = rand() / static_cast(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(RAND_MAX); Tau[i] = rand() / static_cast(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(RAND_MAX); QDot[i] = rand() / static_cast(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)); } diff --git a/3rdparty/rbdl/tests/FloatingBaseTests.cc b/3rdparty/rbdl/tests/FloatingBaseTests.cc index b77b310..ae3d1aa 100644 --- a/3rdparty/rbdl/tests/FloatingBaseTests.cc +++ b/3rdparty/rbdl/tests/FloatingBaseTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - 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); + VectorNd target(7); + target << 0., -9.81, 0., 0., 0., 0., 0.; + + REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, 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); - 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); + REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, 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); - CHECK_CLOSE (-8.8100, QDDot[1], TEST_PREC); - CHECK_CLOSE ( 0.0000, QDDot[2], 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); + target << 0., -8.81, 0., -1., 0., 0., 2.; + + REQUIRE_THAT (target, AllCloseVector(QDDot, TEST_PREC, 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); - CHECK_CLOSE(0., point_velocity[1], TEST_PREC); - CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE(1., point_velocity[1], TEST_PREC); - CHECK_CLOSE(0., point_velocity[2], TEST_PREC); + REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_CLOSE(0., point_velocity[1], TEST_PREC); - CHECK_CLOSE(0., point_velocity[2], TEST_PREC); - + REQUIRE_THAT (Vector3d (-1., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); + REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); + REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (humans_point_velocity.data(), point_world_velocity.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (humans_point_acceleration.data(), point_world_acceleration.data(), 3, TEST_PREC); + REQUIRE_THAT (humans_point_position, AllCloseVector(point_world_position, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (humans_point_velocity, AllCloseVector(point_world_velocity, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (humans_point_acceleration, AllCloseVector(point_world_acceleration, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc b/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc index bff003a..63bb687 100644 --- a/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc +++ b/3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include "rbdl/rbdl.h" #include @@ -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) { -// while(angle > M_PI) { -// angle -= 2. * M_PI; -// } -// while(angle <= -M_PI) { -// angle += 2. * M_PI; -// } -// return angle; -// } +static double inRange(double angle) { + while(angle > M_PI) { + angle -= 2. * M_PI; + } + while(angle <= -M_PI) { + angle += 2. * M_PI; + } + return angle; +} - -TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) { +TEST_CASE (__FILE__"_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 +#include "rbdl_tests.h" #include @@ -75,10 +75,10 @@ struct ImpulsesFixture { ConstraintSet constraint_set; }; -TEST_FIXTURE(ImpulsesFixture, 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.); +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.); constraint_set.Bind (*model); @@ -109,13 +109,13 @@ 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) { - 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.); +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.); constraint_set.Bind (*model); @@ -152,13 +152,13 @@ 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) { - 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.); +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.); constraint_set.Bind (*model); @@ -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); - CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_rangespace.data(), 3, TEST_PREC); + REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_rangespace, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_rangespace, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_nullspace.data(), 3, TEST_PREC); + REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_nullspace, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_nullspace, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/InverseDynamicsTests.cc b/3rdparty/rbdl/tests/InverseDynamicsTests.cc index 943bd8d..66e6e1d 100644 --- a/3rdparty/rbdl/tests/InverseDynamicsTests.cc +++ b/3rdparty/rbdl/tests/InverseDynamicsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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 diff --git a/3rdparty/rbdl/tests/InverseKinematicsTests.cc b/3rdparty/rbdl/tests/InverseKinematicsTests.cc index a50065a..348b489 100644 --- a/3rdparty/rbdl/tests/InverseKinematicsTests.cc +++ b/3rdparty/rbdl/tests/InverseKinematicsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); + REQUIRE_THAT (target_position1, AllCloseVector(result_position1, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position2, AllCloseVector(result_position2, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position3, AllCloseVector(result_position3, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position4, AllCloseVector(result_position4, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); + REQUIRE_THAT (target_orientation1, AllCloseMatrix(result_orientation1, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation2, AllCloseMatrix(result_orientation2, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation3, AllCloseMatrix(result_orientation3, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation4, AllCloseMatrix(result_orientation4, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (target_position2.data(), result_position2.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position3.data(), result_position3.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position4.data(), result_position4.data(), 3, TEST_PREC); - CHECK_ARRAY_CLOSE (target_position5.data(), result_position5.data(), 3, TEST_PREC); - - CHECK_ARRAY_CLOSE (target_orientation1.data(), result_orientation1.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation2.data(), result_orientation2.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation3.data(), result_orientation3.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation4.data(), result_orientation4.data(), 9, TEST_PREC); - CHECK_ARRAY_CLOSE (target_orientation5.data(), result_orientation5.data(), 9, TEST_PREC); + REQUIRE_THAT (target_position1, AllCloseVector(result_position1, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position2, AllCloseVector(result_position2, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position3, AllCloseVector(result_position3, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position4, AllCloseVector(result_position4, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_position5, AllCloseVector(result_position5, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation1, AllCloseMatrix(result_orientation1, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation2, AllCloseMatrix(result_orientation2, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation3, AllCloseMatrix(result_orientation3, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation4, AllCloseMatrix(result_orientation4, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (target_orientation5, AllCloseMatrix(result_orientation5, TEST_PREC, TEST_PREC)); } diff --git a/3rdparty/rbdl/tests/KinematicsTests.cc b/3rdparty/rbdl/tests/KinematicsTests.cc index e939431..6b904be 100644 --- a/3rdparty/rbdl/tests/KinematicsTests.cc +++ b/3rdparty/rbdl/tests/KinematicsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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 ); - CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); - CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, 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 (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 ); - CHECK_ARRAY_CLOSE (Vector3d (0., 1., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); - CHECK_ARRAY_CLOSE (Vector3d (-1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, 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 (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (-1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 ); - CHECK_ARRAY_CLOSE (Vector3d (0.707106781186547, -0.707106781186547, 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, 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 ); - 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 (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 )); + REQUIRE_THAT (Vector3d (sqrt(2.0), 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 ); - CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); - CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, 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., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 ); - CHECK_ARRAY_CLOSE (Vector3d (1., 0., 0.), CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), 3, TEST_PREC ); - CHECK_ARRAY_CLOSE (Vector3d (1., 1., 0.),CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), 3, 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 (0., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_a_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 0., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_b_id, Vector3d (0., 0., 0.), true), TEST_PREC, TEST_PREC )); + REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), true), TEST_PREC, 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 ( - Vector3d (1., 2., 0.), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (1., 2., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.)), TEST_PREC, 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 ( - Vector3d (1., 1., 0.).data(), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false).data(), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC)); - CHECK_ARRAY_CLOSE ( - Vector3d (0., 1., 0.).data(), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false).data(), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (0., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, 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 ( - Vector3d (1., 1., 0.), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC)); - CHECK_ARRAY_CLOSE ( - Vector3d (2., 1., 0.), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (2., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, 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 ( - Vector3d (-1., 1., 0.), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (-1., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 0., 0.), false), TEST_PREC, TEST_PREC)); - CHECK_ARRAY_CLOSE ( - Vector3d (-2., 1., 0.), - CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), - 3, TEST_PREC - ); + REQUIRE_THAT (Vector3d (-2., 1., 0.), AllCloseVector(CalcBodyToBaseCoordinates(*model, Q, body_c_id, Vector3d (0., 1., 0.), false), TEST_PREC, 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 ( - point_velocity_ref.data(), - point_velocity.data(), - 3, TEST_PREC - ); + REQUIRE_THAT (point_velocity_ref, AllCloseVector(point_velocity, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(KinematicsFixture, TestInverseKinematicSimple) { +TEST_CASE_METHOD(KinematicsFixture, __FILE__"_TestInverseKinematicSimple", "") { std::vector body_ids; std::vector body_points; std::vector 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 body_ids; std::vector body_points; std::vector 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 body_ids; std::vector body_points; std::vector 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)); } diff --git a/3rdparty/rbdl/tests/LoopConstraintsTests.cc b/3rdparty/rbdl/tests/LoopConstraintsTests.cc index 36ca6d4..8489f3c 100644 --- a/3rdparty/rbdl/tests/LoopConstraintsTests.cc +++ b/3rdparty/rbdl/tests/LoopConstraintsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include "rbdl/rbdl.h" #include @@ -266,20 +266,17 @@ struct SliderCrank3D { = (CalcBodyWorldOrientation(model,q,id_p).transpose()*X_p.E).transpose() * CalcBodyWorldOrientation(model,q,id_s).transpose()*X_s.E; - CHECK_CLOSE(rot_ps(0,0), -1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,1), 1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,2), -1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(0,1), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(0,2), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,0), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,2), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,0), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,1), 0.0, TEST_PREC); - - CHECK_ARRAY_CLOSE( - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r), - 3, TEST_PREC); + REQUIRE (rot_ps(0,0) - 1. < TEST_PREC); + REQUIRE (rot_ps(1,1) - 1. < TEST_PREC); + REQUIRE (rot_ps(2,2) - 1. < TEST_PREC); + REQUIRE (rot_ps(0,1) < TEST_PREC); + REQUIRE (rot_ps(0,2) < TEST_PREC); + REQUIRE (rot_ps(1,0) < TEST_PREC); + REQUIRE (rot_ps(1,2) < TEST_PREC); + REQUIRE (rot_ps(2,0) < TEST_PREC); + REQUIRE (rot_ps(2,1) < TEST_PREC); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC); } Model model; @@ -368,20 +365,18 @@ struct SliderCrank3DSphericalJoint { Matrix3d rot_ps = (CalcBodyWorldOrientation(model,q,id_p).transpose()*X_p.E).transpose() * CalcBodyWorldOrientation(model,q,id_s).transpose()*X_s.E; - CHECK_CLOSE(rot_ps(0,0), -1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,1), 1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,2), -1.0, TEST_PREC); - CHECK_CLOSE(rot_ps(0,1), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(0,2), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,0), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(1,2), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,0), 0.0, TEST_PREC); - CHECK_CLOSE(rot_ps(2,1), 0.0, TEST_PREC); - CHECK_ARRAY_CLOSE( - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r), - 3, TEST_PREC); + REQUIRE (rot_ps(0,0) - 1. < TEST_PREC); + REQUIRE (rot_ps(1,1) - 1. < TEST_PREC); + REQUIRE (rot_ps(2,2) - 1. < TEST_PREC); + REQUIRE (rot_ps(0,1) < TEST_PREC); + REQUIRE (rot_ps(0,2) < TEST_PREC); + REQUIRE (rot_ps(1,0) < TEST_PREC); + REQUIRE (rot_ps(1,2) < TEST_PREC); + REQUIRE (rot_ps(2,0) < TEST_PREC); + REQUIRE (rot_ps(2,1) < TEST_PREC); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC); } @@ -400,7 +395,7 @@ struct SliderCrank3DSphericalJoint { }; -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageConstraintErrors", "") { VectorNd err = VectorNd::Zero(cs.size()); Vector3d pos1; Vector3d pos2; @@ -417,9 +412,7 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { CalcConstraintsPositionError(model, q, cs, err); - CHECK_CLOSE(0., err[0], TEST_PREC); - CHECK_CLOSE(0., err[1], TEST_PREC); - CHECK_CLOSE(0., err[2], TEST_PREC); + REQUIRE_THAT(Vector3d (0., 0., 0.), AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = M_PI * 3 / 4; @@ -434,14 +427,12 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; posErr = rot_p.transpose() * (pos2 - pos1); - assert(std::fabs(posErr[1]) < TEST_PREC); - assert(std::fabs(posErr[2]) < TEST_PREC); + REQUIRE (std::fabs(posErr[1]) < TEST_PREC); + REQUIRE (std::fabs(posErr[2]) < TEST_PREC); CalcConstraintsPositionError(model, q, cs, err); - CHECK_CLOSE(posErr[0], err[0], TEST_PREC); - CHECK_CLOSE(0., err[1], TEST_PREC); - CHECK_CLOSE(angleErr, err[2], TEST_PREC); + REQUIRE_THAT (Vector3d (posErr[0], 0., angleErr), AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = 0.; @@ -458,9 +449,7 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { CalcConstraintsPositionError(model, q, cs, err); - CHECK_CLOSE(posErr[0], err[0], TEST_PREC); - CHECK_CLOSE(posErr[1], err[1], TEST_PREC); - CHECK_CLOSE(angleErr, err[2], TEST_PREC); + REQUIRE_THAT(Vector3d (posErr[0], posErr[1], angleErr), AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = 0.8; @@ -477,12 +466,10 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) { CalcConstraintsPositionError(model, q, cs, err); - CHECK_CLOSE(posErr[0], err[0], TEST_PREC); - CHECK_CLOSE(posErr[1], err[1], TEST_PREC); - CHECK_CLOSE(angleErr, err[2], TEST_PREC); + REQUIRE_THAT(Vector3d (posErr[0], posErr[1], angleErr), AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageConstraintJacobian", "") { MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); VectorNd err(VectorNd::Zero(cs.size())); VectorNd errRef(VectorNd::Zero(cs.size())); @@ -493,8 +480,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { q[2] = 0.; q[3] = 0.; q[4] = 0.; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = -1.; @@ -502,14 +489,14 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { qd[2] = -1.; qd[3] = -1.; qd[4] = 0.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Both arms of the 4-bar laying on the y-axis q[0] = 0.5 * M_PI; @@ -517,8 +504,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { q[2] = 0.5 * M_PI; q[3] = 0.; q[4] = 0.; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = -1.; @@ -526,14 +513,14 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { qd[2] = -1.; qd[3] = -1.; qd[4] = 0.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Arms symmetric wrt y axis. q[0] = M_PI * 3 / 4; @@ -541,8 +528,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { q[2] = M_PI - q[0]; q[3] = -q[1]; q[4] = q[0] + q[1] - q[2] - q[3]; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = -1.; @@ -550,17 +537,17 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) { qd[2] = -2.; qd[3] = +1.; qd[4] = -1.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageConstraintsVelocityErrors", "") { VectorNd errd(VectorNd::Zero(cs.size())); VectorNd errdRef(VectorNd::Zero(cs.size())); MatrixNd G(cs.size(), model.dof_count); @@ -571,8 +558,9 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { q[2] = M_PI - q[0]; q[3] = -q[1]; q[4] = q[0] + q[1] - q[2] - q[3]; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = -1.; @@ -583,7 +571,7 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { CalcConstraintsVelocityError(model, q, qd, cs, errd); - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); // Invalid velocities. qd[0] = -1.; @@ -595,10 +583,10 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) { CalcConstraintsVelocityError(model, q, qd, cs, errd); CalcConstraintsJacobian(model, q, cs, G); errdRef = G * qd; - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageQAssembly", "") { VectorNd weights(q.size()); VectorNd err(cs.size()); VectorNd errRef(VectorNd::Zero(cs.size())); @@ -615,7 +603,7 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { qRef[2] = M_PI - qRef[0]; qRef[3] = -qRef[1]; qRef[4] = qRef[0] + qRef[1] - qRef[2] - qRef[3]; - assert(qRef[0] + qRef[1] - qRef[2] - qRef[3] - qRef[4] == 0.); + REQUIRE (qRef[0] + qRef[1] - qRef[2] - qRef[3] - qRef[4] == 0.); bool success; @@ -626,12 +614,11 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); - CHECK_CLOSE(qInit[0], q[0], TEST_PREC); - CHECK_CLOSE(qInit[2], q[2], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (inRange(q[0] + q[1]), IsClose(inRange(q[2] + q[3] + q[4]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[0], IsClose(q[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[2], IsClose(q[2], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[0] = qRef[0]; @@ -643,13 +630,12 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); - CHECK_CLOSE(qInit[0], q[0], TEST_PREC); - CHECK_CLOSE(qInit[2], q[2], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE(success); + REQUIRE_THAT (CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (inRange(q[0] + q[1]), IsClose(inRange(q[2] + q[3] + q[4]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[0], IsClose(q[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[2], IsClose(q[2], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[0] = qRef[0] - 0.2; @@ -661,13 +647,12 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); - CHECK_CLOSE(qInit[0], q[0], TEST_PREC); - CHECK_CLOSE(qInit[2], q[2], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT (CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (inRange(q[0] + q[1]), IsClose(inRange(q[2] + q[3] + q[4]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[0], IsClose(q[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[2], IsClose(q[2], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[0] = qRef[0] + 0.01; @@ -679,16 +664,15 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC); - CHECK_CLOSE(qInit[0], q[0], TEST_PREC); - CHECK_CLOSE(qInit[2], q[2], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT (CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (inRange(q[0] + q[1]), IsClose(inRange(q[2] + q[3] + q[4]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[0], IsClose(q[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qInit[2], IsClose(q[2], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageQDotAssembly", "") { VectorNd weights(q.size()); weights[0] = 1.; @@ -702,8 +686,9 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) { q[2] = M_PI - q[0]; q[3] = -q[1]; q[4] = q[0] + q[1] - q[2] - q[3]; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); VectorNd qdInit = VectorNd::Zero(q.size()); @@ -719,15 +704,14 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) { VectorNd errRef(VectorNd::Zero(cs.size())); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - - CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); - CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); - CHECK_CLOSE(qdInit[2], qd[2], TEST_PREC); + + REQUIRE_THAT (CalcPointVelocity6D(model, q, qd, idB2, X_p.r), AllCloseVector(CalcPointVelocity6D(model, q, qd, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qdInit[0], IsClose(qd[0], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (qdInit[2], IsClose(qd[2], TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_TestFourBarLinkageForwardDynamics", "") { VectorNd qddDirect; VectorNd qddNullSpace; @@ -742,8 +726,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { q[2] = 0.; q[3] = 0.; q[4] = 0.; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -751,8 +735,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { qd[2] = 0.; qd[3] = 0.; qd[4] = 0.; - assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); - assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + REQUIRE (qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + REQUIRE ((CalcPointVelocity(model, q, qd, idB2, X_p.r) - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); tau[0] = 1.; @@ -764,18 +748,16 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { qddDirect = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); qddNullSpace = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); #endif // Configuration 2. @@ -785,8 +767,9 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { q[2] = M_PI - q[0]; q[3] = -q[1]; q[4] = q[0] + q[1] - q[2] - q[3]; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = -1.; @@ -794,8 +777,8 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { qd[2] = -2.; qd[3] = +1.; qd[4] = -1.; - assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); - assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + REQUIRE (qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.); + REQUIRE ((CalcPointVelocity(model, q, qd, idB2, X_p.r) - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); tau[0] = 1.; @@ -807,18 +790,16 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { qddDirect = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT(CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); qddNullSpace = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); // Note: // The Range Space Sparse method can't be used because the H matrix has a 0 @@ -833,7 +814,7 @@ TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) { // in a singular configuration. } -TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { +TEST_CASE_METHOD (FourBarLinkage, __FILE__"_FourBarLinkageImpulse", "") { VectorNd qdPlusDirect(qd.size()); VectorNd qdPlusRangeSpaceSparse(qd.size()); VectorNd qdPlusNullSpace(qd.size()); @@ -844,8 +825,8 @@ TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { q[2] = M_PI - q[0]; q[3] = -q[1]; q[4] = q[0] + q[1] - q[2] - q[3]; - assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[0] + q[1] - q[2] - q[3] - q[4] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); cs.v_plus[0] = 1.; @@ -855,7 +836,7 @@ TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); - CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + REQUIRE_THAT(cs.v_plus, AllCloseVector(errd, TEST_PREC, TEST_PREC)); cs.v_plus[0] = 0.; cs.v_plus[1] = 0.; @@ -868,12 +849,12 @@ TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); - CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + REQUIRE_THAT(cs.v_plus, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DConstraintErrors", "") { VectorNd err(VectorNd::Zero(cs.size())); VectorNd errRef(VectorNd::Zero(cs.size())); Vector3d pos_p; @@ -885,7 +866,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) { // Test in zero position. CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in another configurations. @@ -909,10 +890,10 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) { errRef.block<3,1>(0,0) = pos_s - pos_p; errRef[3] = rotationVec[2]; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintJacobian) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DConstraintJacobian", "") { MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); // Test in zero position. @@ -923,10 +904,10 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintJacobian) { VectorNd errRef(VectorNd::Zero(cs.size())); VectorNd err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DConstraintsVelocityErrors", "") { VectorNd errd(VectorNd::Zero(cs.size())); VectorNd errdRef(VectorNd::Zero(cs.size())); MatrixNd G(cs.size(), model.dof_count); @@ -948,7 +929,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) { qInit[4] = 0.1; success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); - CHECK_EQUAL(success, true); + REQUIRE (success); // Some random velocity. qd[0] = -0.2; @@ -961,10 +942,10 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) { CalcConstraintsJacobian(model, q, cs, G); errdRef = G * qd; - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQ) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DAssemblyQ", "") { VectorNd weights(q.size()); VectorNd qInit(q.size()); @@ -995,12 +976,12 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQ) { rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; rot_ps = rot_p.transpose() * rot_s; - CHECK(success); - CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); - CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT (pos_p, AllCloseVector(pos_s, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(rot_ps(0,1) - rot_ps(1,0), TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DAssemblyQDot", "") { VectorNd qWeights(q.size()); VectorNd qdWeights(q.size()); VectorNd qInit(q.size()); @@ -1036,7 +1017,8 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) { qdInit[4] = 0.1 * M_PI; success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); - CHECK_EQUAL(success, true); + + REQUIRE (success); CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); @@ -1044,12 +1026,12 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) { vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + REQUIRE_THAT(vel_p[i], IsClose(vel_s[i], TEST_PREC, TEST_PREC)); } - CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); + REQUIRE_THAT(qdInit[0], IsClose(qd[0], TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DForwardDynamics", "") { VectorNd qWeights(q.size()); VectorNd qdWeights(q.size()); VectorNd qInit(q.size()); @@ -1077,7 +1059,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); @@ -1086,7 +1068,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT(acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); @@ -1095,7 +1077,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } #endif @@ -1128,24 +1110,21 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { qdInit.setZero(); success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); - CHECK_EQUAL(success, true); + + REQUIRE (success); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); Matrix3d rot_ps = (CalcBodyWorldOrientation(model, q, id_p).transpose()*X_p.E).transpose() * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; - CHECK_CLOSE(rot_ps(0,1), rot_ps(0,1), TEST_PREC); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC); + REQUIRE (rot_ps(0,1) - rot_ps(0,1) < TEST_PREC); + REQUIRE ((CalcPointVelocity6D(model, q, qd, id_p, X_p.r) + -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC); - CHECK_ARRAY_CLOSE( - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - 3, TEST_PREC); - - CHECK_ARRAY_CLOSE( - CalcPointVelocity6D(model, q, qd, id_p, X_p.r), - CalcPointVelocity6D(model, q, qd, id_p, X_p.r), - 3, TEST_PREC); // Test with non-zero q and qdot. @@ -1155,7 +1134,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); @@ -1164,7 +1143,7 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); @@ -1173,11 +1152,11 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) { acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } } -TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { +TEST_CASE_METHOD (SliderCrank3D, __FILE__"_TestSliderCrank3DImpulse", "") { VectorNd qdPlusDirect(qd.size()); VectorNd qdPlusRangeSpaceSparse(qd.size()); VectorNd qdPlusNullSpace(qd.size()); @@ -1200,7 +1179,8 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { qInit[4] = 0.1; bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC); - CHECK_EQUAL(success, true); + + REQUIRE (success); cs.v_plus[0] = 1.; cs.v_plus[1] = 2.; @@ -1209,20 +1189,20 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdDirect, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs , qdPlusRangeSpaceSparse); CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs , errdSpaceSparse); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdSpaceSparse, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); - CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdNullSpace, TEST_PREC, TEST_PREC)); cs.v_plus[0] = 0.; cs.v_plus[1] = 0.; @@ -1236,24 +1216,23 @@ TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); - CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdDirect, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs , qdPlusRangeSpaceSparse); CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs , errdSpaceSparse); - CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + REQUIRE_THAT(cs.v_plus, AllCloseVector(errdSpaceSparse, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); - CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + REQUIRE_THAT(cs.v_plus, AllCloseVector(errdNullSpace, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage - , TestFloatingFourBarLinkageConstraintErrors) { - +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageConstraintErrors", "") { VectorNd err = VectorNd::Zero(cs.size()); Vector3d pos0; Vector3d pos1; @@ -1274,12 +1253,8 @@ TEST_FIXTURE(FloatingFourBarLinkage CalcConstraintsPositionError(model, q, cs, err); - CHECK_CLOSE(0., err[0], TEST_PREC); - CHECK_CLOSE(0., err[1], TEST_PREC); - CHECK_CLOSE(0., err[2], TEST_PREC); - CHECK_CLOSE(0., err[3], TEST_PREC); - CHECK_CLOSE(0., err[4], TEST_PREC); - CHECK_CLOSE(0., err[5], TEST_PREC); + VectorNd target(6); + REQUIRE_THAT(target, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = 1.; @@ -1298,15 +1273,13 @@ TEST_FIXTURE(FloatingFourBarLinkage rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E; posErr = rot_p.transpose() * (pos2 - pos1); - assert(std::fabs(posErr[1]) < TEST_PREC); - assert(std::fabs(posErr[2]) < TEST_PREC); + REQUIRE (std::fabs(posErr[1]) < TEST_PREC); + REQUIRE (std::fabs(posErr[2]) < TEST_PREC); CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); - CHECK_CLOSE(posErr[0], err[3], TEST_PREC); - CHECK_CLOSE(0., err[4], TEST_PREC); - CHECK_CLOSE(angleErr, err[5], TEST_PREC); + REQUIRE_THAT (Vector3d(1.,2.,3.), AllCloseVector(pos0, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d(posErr[0], 0., angleErr), AllCloseVector(Vector3d(err[3], err[4], err[5]), TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = 1.; @@ -1327,10 +1300,8 @@ TEST_FIXTURE(FloatingFourBarLinkage CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); - CHECK_CLOSE(posErr[0], err[3], TEST_PREC); - CHECK_CLOSE(posErr[1], err[4], TEST_PREC); - CHECK_CLOSE(angleErr, err[5], TEST_PREC); + REQUIRE_THAT (Vector3d(1.,2.,3.), AllCloseVector(pos0, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d(posErr[0], posErr[1], angleErr), AllCloseVector(Vector3d(err[3], err[4], err[5]), TEST_PREC, TEST_PREC)); // Test in non-zero position. q[0] = 1.; @@ -1351,15 +1322,11 @@ TEST_FIXTURE(FloatingFourBarLinkage CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC); - CHECK_CLOSE(posErr[0], err[3], TEST_PREC); - CHECK_CLOSE(posErr[1], err[4], TEST_PREC); - CHECK_CLOSE(angleErr, err[5], TEST_PREC); + REQUIRE_THAT (Vector3d(1.,2.,3.), AllCloseVector(pos0, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (Vector3d(posErr[0], posErr[1], angleErr), AllCloseVector(Vector3d(err[3], err[4], err[5]), TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage - , TestFloatingFourBarLinkageConstraintJacobian) { - +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageConstraintJacobian", "") { MatrixNd G(MatrixNd::Zero(cs.size(), q.size())); VectorNd err(VectorNd::Zero(cs.size())); VectorNd errRef(VectorNd::Zero(cs.size())); @@ -1373,8 +1340,8 @@ TEST_FIXTURE(FloatingFourBarLinkage q[5] = 0.; q[6] = 0.; q[7] = 0.; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1385,14 +1352,14 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[5] = -1.; qd[6] = -1.; qd[7] = 0.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Both arms of the 4-bar laying on the y-axis q[0] = 0.; @@ -1403,8 +1370,9 @@ TEST_FIXTURE(FloatingFourBarLinkage q[5] = 0.5 * M_PI; q[6] = 0.; q[7] = 0.; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1415,14 +1383,14 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[5] = -1.; qd[6] = -1.; qd[7] = 0.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Arms symmetric wrt y axis. q[0] = 1.; @@ -1433,8 +1401,9 @@ TEST_FIXTURE(FloatingFourBarLinkage q[5] = M_PI - q[3]; q[6] = -q[4]; q[7] = q[3] + q[4] - q[5] - q[6]; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1445,19 +1414,17 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[5] = -2.; qd[6] = +1.; qd[7] = -1.; - assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) + REQUIRE ((CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage - , TestFloatingFourBarLinkageConstraintsVelocityErrors) { - +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageConstraintsVelocityErrors", "") { VectorNd errd(VectorNd::Zero(cs.size())); VectorNd errdRef(VectorNd::Zero(cs.size())); MatrixNd G(cs.size(), model.dof_count); @@ -1472,8 +1439,9 @@ TEST_FIXTURE(FloatingFourBarLinkage q[6] = -q[4]; q[7] = q[3] + q[4] - q[5] - q[6]; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1486,7 +1454,7 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[7] = -1.; CalcConstraintsVelocityError(model, q, qd, cs, errd); - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); // Invalid velocities. qd[0] = -1.; @@ -1501,10 +1469,10 @@ TEST_FIXTURE(FloatingFourBarLinkage CalcConstraintsVelocityError(model, q, qd, cs, errd); CalcConstraintsJacobian(model, q, cs, G); errdRef = G * qd; - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageQAssembly", "") { VectorNd weights(q.size()); VectorNd err(cs.size()); VectorNd errRef(VectorNd::Zero(cs.size())); @@ -1528,8 +1496,9 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { qRef[6] = -qRef[4]; qRef[7] = qRef[3] + qRef[4] - qRef[5] - qRef[6]; - assert(qRef[3] + qRef[4] - qRef[5] - qRef[6] - qRef[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, qRef, idB2, X_p.r) + + REQUIRE (qRef[3] + qRef[4] - qRef[5] - qRef[6] - qRef[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, qRef, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, qRef, idB5, X_s.r)).norm() < TEST_PREC); bool success; @@ -1541,12 +1510,11 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); - CHECK_CLOSE(qInit[3], q[3], TEST_PREC); - CHECK_CLOSE(qInit[5], q[5], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(inRange(q[3] + q[4]), IsClose(inRange(q[5] + q[6] + q[7]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[3], IsClose(q[3], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[5], IsClose(q[5], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[3] = qRef[3]; @@ -1558,13 +1526,12 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); - CHECK_CLOSE(qInit[3], q[3], TEST_PREC); - CHECK_CLOSE(qInit[5], q[5], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(inRange(q[3] + q[4]), IsClose(inRange(q[5] + q[6] + q[7]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[3], IsClose(q[3], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[5], IsClose(q[5], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[3] = qRef[3] - 0.2; @@ -1576,13 +1543,12 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); - CHECK_CLOSE(qInit[3], q[3], TEST_PREC); - CHECK_CLOSE(qInit[5], q[5], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(inRange(q[3] + q[4]), IsClose(inRange(q[5] + q[6] + q[7]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[3], IsClose(q[3], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[5], IsClose(q[5], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Perturbed initial guess. qInit[3] = qRef[3] + 0.01; @@ -1594,16 +1560,15 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) { success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12); CalcConstraintsPositionError(model, q, cs, err); - CHECK(success); - CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC); - CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC); - CHECK_CLOSE(qInit[3], q[3], TEST_PREC); - CHECK_CLOSE(qInit[5], q[5], TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r), AllCloseVector(CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(inRange(q[3] + q[4]), IsClose(inRange(q[5] + q[6] + q[7]), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[3], IsClose(q[3], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qInit[5], IsClose(q[5], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) { +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageQDotAssembly", "") { VectorNd weights(q.size()); weights[0] = 0.; @@ -1624,8 +1589,9 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) { q[6] = -q[4]; q[7] = q[3] + q[4] - q[5] - q[6]; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); VectorNd qdInit = VectorNd::Zero(q.size()); @@ -1644,17 +1610,14 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) { VectorNd errRef(VectorNd::Zero(cs.size())); CalcConstraintsJacobian(model, q, cs, G); err = G * qd; - - CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r) - , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); - CHECK_CLOSE(qdInit[3], qd[3], TEST_PREC); - CHECK_CLOSE(qdInit[5], qd[5], TEST_PREC); + + REQUIRE_THAT(CalcPointVelocity6D(model, q, qd, idB2, X_p.r), AllCloseVector(CalcPointVelocity6D(model, q, qd, idB5, X_s.r), TEST_PREC, TEST_PREC)); + REQUIRE_THAT(errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qdInit[3], IsClose(qd[3], TEST_PREC, TEST_PREC)); + REQUIRE_THAT(qdInit[5], IsClose(qd[5], TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(FloatingFourBarLinkage - , TestFloatingFourBarLinkageForwardDynamics) { - +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageForwardDynamics", "") { VectorNd qddDirect; VectorNd qddNullSpace; @@ -1672,8 +1635,9 @@ TEST_FIXTURE(FloatingFourBarLinkage q[5] = 0.; q[6] = 0.; q[7] = 0.; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1684,8 +1648,8 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[5] = 0.; qd[6] = 0.; qd[7] = 0.; - assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); - assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + REQUIRE (qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + REQUIRE ((CalcPointVelocity(model, q, qd, idB2, X_p.r) - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); tau[0] = 0.; @@ -1700,18 +1664,16 @@ TEST_FIXTURE(FloatingFourBarLinkage qddDirect = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); qddNullSpace = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); #endif // Configuration 2. @@ -1724,8 +1686,9 @@ TEST_FIXTURE(FloatingFourBarLinkage q[6] = -q[4]; q[7] = q[3] + q[4] - q[5] - q[6]; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); qd[0] = 0.; @@ -1736,8 +1699,8 @@ TEST_FIXTURE(FloatingFourBarLinkage qd[5] = -2.; qd[6] = +1.; qd[7] = -1.; - assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); - assert((CalcPointVelocity(model, q, qd, idB2, X_p.r) + REQUIRE (qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.); + REQUIRE ((CalcPointVelocity(model, q, qd, idB2, X_p.r) - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC); tau[0] = 0.; @@ -1752,21 +1715,19 @@ TEST_FIXTURE(FloatingFourBarLinkage qddDirect = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); qddNullSpace = VectorNd::Zero(q.size()); ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace); - CHECK_ARRAY_CLOSE - (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r) - , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r) - , 6, TEST_PREC); + REQUIRE_THAT (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r), + AllCloseVector(CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r), TEST_PREC, TEST_PREC) + ); } -TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { +TEST_CASE_METHOD (FloatingFourBarLinkage, __FILE__"_TestFloatingFourBarLinkageImpulse", "") { VectorNd qdPlusDirect(qd.size()); VectorNd qdPlusRangeSpaceSparse(qd.size()); VectorNd qdPlusNullSpace(qd.size()); @@ -1781,8 +1742,8 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { q[6] = -q[4]; q[7] = q[3] + q[4] - q[5] - q[6]; - assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.); - assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) + REQUIRE (q[3] + q[4] - q[5] - q[6] - q[7] == 0.); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC); cs.v_plus[0] = 1.; @@ -1795,7 +1756,7 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); - CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + REQUIRE_THAT(cs.v_plus, AllCloseVector(errd, TEST_PREC, TEST_PREC)); cs.v_plus[0] = 0.; cs.v_plus[1] = 0.; @@ -1811,11 +1772,10 @@ TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) { ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd); - CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (cs.v_plus, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointConstraintErrors) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointConstraintErrors", "") { VectorNd err(VectorNd::Zero(cs.size())); VectorNd errRef(VectorNd::Zero(cs.size())); Vector3d pos_p; @@ -1827,7 +1787,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint // Test in zero position. CalcConstraintsPositionError(model, q, cs, err); - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); // Test in another configuration. Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25*M_PI,0.01,0.01)); @@ -1849,11 +1809,10 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint errRef.block<3,1>(0,0) = pos_s - pos_p; errRef[3] = rotationVec[2]; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointConstraintJacobian) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointConstraintJacobian", "") { MatrixNd G(MatrixNd::Zero(cs.size(), model.dof_count)); // Test in zero position. @@ -1864,11 +1823,10 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint VectorNd errRef(VectorNd::Zero(cs.size())); VectorNd err = G * qd; - CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC); + REQUIRE_THAT (errRef, AllCloseVector(err, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointConstraintsVelocityErrors) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointConstraintsVelocityErrors", "") { VectorNd errd(VectorNd::Zero(cs.size())); VectorNd errdRef(VectorNd::Zero(cs.size())); MatrixNd G(cs.size(), model.dof_count); @@ -1889,7 +1847,8 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint model.SetQuaternion(id_s, quat, qInit); success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); - CHECK_EQUAL(success, true); + + REQUIRE (success); // Some random velocity. qd[0] = -0.2; @@ -1902,11 +1861,10 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint CalcConstraintsJacobian(model, q, cs, G); errdRef = G * qd; - CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC); + REQUIRE_THAT (errdRef, AllCloseVector(errd, TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointAssemblyQ) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointAssemblyQ", "") { VectorNd weights(model.dof_count); VectorNd qInit(model.q_size); @@ -1936,13 +1894,12 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; rot_ps = rot_p.transpose() * rot_s; - CHECK(success); - CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC); - CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC); + REQUIRE (success); + REQUIRE_THAT (pos_p, AllCloseVector(pos_s, TEST_PREC, TEST_PREC)); + REQUIRE_THAT (0., IsClose(rot_ps(0,1) - rot_ps(1,0), TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointAssemblyQDot) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointAssemblyQDot", "") { VectorNd qWeights(model.dof_count); VectorNd qdWeights(model.dof_count); VectorNd qInit(model.q_size); @@ -1977,7 +1934,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint qdInit[4] = 0.1 * M_PI; success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); - CHECK_EQUAL(success, true); + REQUIRE (success); CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); @@ -1985,13 +1942,12 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC); + REQUIRE_THAT (vel_p[i], IsClose(vel_s[i], TEST_PREC, TEST_PREC)); } - CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC); + REQUIRE_THAT (qdInit[0], IsClose(qd[0], TEST_PREC, TEST_PREC)); } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointForwardDynamics) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointForwardDynamics", "") { VectorNd qWeights(model.dof_count); VectorNd qdWeights(model.dof_count); @@ -2020,7 +1976,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); @@ -2029,7 +1985,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT(acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); @@ -2038,7 +1994,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } #endif @@ -2070,22 +2026,20 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint qdInit.setZero(); success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); - CHECK_EQUAL(success, true); + + REQUIRE (success); + CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights); Matrix3d rot_ps = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose() * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E; - CHECK_CLOSE(rot_ps(0,1), rot_ps(0,1), TEST_PREC); - CHECK_ARRAY_CLOSE( - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r), - 3, TEST_PREC); - CHECK_ARRAY_CLOSE( - CalcPointVelocity6D(model, q, qd, id_p, X_p.r), - CalcPointVelocity6D(model, q, qd, id_p, X_p.r), - 3, TEST_PREC); + REQUIRE ((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r) + - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC); + REQUIRE (rot_ps(0,1) - rot_ps(0,1) < TEST_PREC); + REQUIRE ((CalcPointVelocity6D(model, q, qd, id_p, X_p.r) + -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC); // Test with non-zero q and qdot. @@ -2095,7 +2049,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT (acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd); @@ -2104,7 +2058,7 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT(acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd); @@ -2113,12 +2067,11 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r); for(size_t i = 2; i < 6; ++i) { - CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC); + REQUIRE_THAT(acc_p[i], IsClose(acc_s[i], TEST_PREC, TEST_PREC)); } } -TEST_FIXTURE(SliderCrank3DSphericalJoint - , TestSliderCrank3DSphericalJointImpulse) { +TEST_CASE_METHOD (SliderCrank3DSphericalJoint, __FILE__"_TestSliderCrank3DSphericalJointImpulse", "") { VectorNd qdPlusDirect(model.dof_count); VectorNd qdPlusRangeSpaceSparse(model.dof_count); @@ -2141,7 +2094,8 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint model.SetQuaternion(id_s, quat, qInit); bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800); - CHECK_EQUAL(success, true); + + REQUIRE (success); cs.v_plus[0] = 1.; cs.v_plus[1] = 2.; @@ -2150,20 +2104,20 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdDirect, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs , qdPlusRangeSpaceSparse); CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs , errdSpaceSparse); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdSpaceSparse, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); - CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdNullSpace, TEST_PREC, TEST_PREC)); cs.v_plus[0] = 0.; cs.v_plus[1] = 0.; @@ -2176,23 +2130,23 @@ TEST_FIXTURE(SliderCrank3DSphericalJoint ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect); CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdDirect, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs , qdPlusRangeSpaceSparse); CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs , errdSpaceSparse); - - CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC); + + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdSpaceSparse, TEST_PREC, TEST_PREC)); ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace); CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace); - CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC); + REQUIRE_THAT (cs.v_plus, AllCloseVector(errdNullSpace, TEST_PREC, TEST_PREC)); } -TEST(ConstraintCorrectnessTest) { +TEST_CASE (__FILE__"_ConstraintCorrectnessTest", "") { DoublePerpendicularPendulumAbsoluteCoordinates dba = DoublePerpendicularPendulumAbsoluteCoordinates(); DoublePerpendicularPendulumJointCoordinates dbj @@ -2297,10 +2251,10 @@ TEST(ConstraintCorrectnessTest) { //The constraint errors at the position and velocity level //must be zero before the accelerations can be tested. for(unsigned int i=0; i +#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) { - VectorNd myvector_10 = VectorNd::Constant ((size_t) 10, 12.); +TEST_CASE(__FILE__"_QuaternionSlerpNegativeCosHalfTheta", "") { + ClearLogOutput(); - double *test_values = new double[10]; - for (unsigned int i = 0; i < 10; i++) - test_values[i] = 12.; + Quaternion q1 (-0.518934,0.561432,-0.074923,0.640225); + Quaternion q2 (0.54702,-0.564195,0.078871,-0.613379); - CHECK_ARRAY_EQUAL (test_values, myvector_10.data(), 10); - delete[] test_values; + Quaternion s = q1.slerp (0.2021, q2); + Quaternion q_ref (0.0068865, 0.406762, -0.000610507, 0.913655); + + REQUIRE_THAT (s, AllCloseVector(q_ref)); } -TEST (Dynamic_2D_initialize_value) { - MatrixNd mymatrix_10x10 = MatrixNd::Constant (10, 10, 12.); +TEST_CASE(__FILE__"_QuaternionFromMatrixSingularity", "") { + ClearLogOutput(); + Matrix3d m; + m << -1., 0, 0, 0, 1, 0, 0, 0, -1; - double *test_values = new double[10 * 10]; - for (unsigned int i = 0; i < 10; i++) - for (unsigned int j = 0; j < 10; j++) - test_values[i*10 + j] = 12.; + Quaternion q = Quaternion::fromMatrix(m); + Quaternion q_ref (0., 1., 0., 0.); - CHECK_ARRAY_EQUAL (test_values, mymatrix_10x10.data(), 10*10); - 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); + REQUIRE_THAT (q, AllCloseVector(q_ref)); } diff --git a/3rdparty/rbdl/tests/ModelTests.cc b/3rdparty/rbdl/tests/ModelTests.cc index 6439084..be42ef5 100644 --- a/3rdparty/rbdl/tests/ModelTests.cc +++ b/3rdparty/rbdl/tests/ModelTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -28,117 +28,117 @@ struct ModelFixture { Model *model; }; -TEST_FIXTURE(ModelFixture, TestInit) { - CHECK_EQUAL (1u, model->lambda.size()); - CHECK_EQUAL (1u, model->mu.size()); - CHECK_EQUAL (0u, model->dof_count); +TEST_CASE_METHOD(ModelFixture, __FILE__"_TestInit", "") { + REQUIRE (1u == model->lambda.size()); + REQUIRE (1u == model->mu.size()); + REQUIRE (0u == model->dof_count); - CHECK_EQUAL (0u, model->q_size); - CHECK_EQUAL (0u, model->qdot_size); + REQUIRE (0u == model->q_size); + REQUIRE (0u == model->qdot_size); - CHECK_EQUAL (1u, model->v.size()); - CHECK_EQUAL (1u, model->a.size()); + REQUIRE (1u == model->v.size()); + REQUIRE (1u == model->a.size()); - CHECK_EQUAL (1u, model->mJoints.size()); - CHECK_EQUAL (1u, model->S.size()); + REQUIRE (1u == model->mJoints.size()); + REQUIRE (1u == model->S.size()); - CHECK_EQUAL (1u, model->c.size()); - CHECK_EQUAL (1u, model->IA.size()); - CHECK_EQUAL (1u, model->pA.size()); - CHECK_EQUAL (1u, model->U.size()); - CHECK_EQUAL (1u, model->d.size()); - CHECK_EQUAL (1u, model->u.size()); - CHECK_EQUAL (1u, model->Ic.size()); - CHECK_EQUAL (1u, model->I.size()); + REQUIRE (1u == model->c.size()); + REQUIRE (1u == model->IA.size()); + REQUIRE (1u == model->pA.size()); + REQUIRE (1u == model->U.size()); + REQUIRE (1u == model->d.size()); + REQUIRE (1u == model->u.size()); + REQUIRE (1u == model->Ic.size()); + REQUIRE (1u == model->I.size()); - CHECK_EQUAL (1u, model->X_lambda.size()); - CHECK_EQUAL (1u, model->X_base.size()); - CHECK_EQUAL (1u, model->mBodies.size()); + REQUIRE (1u == model->X_lambda.size()); + REQUIRE (1u == model->X_base.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); - CHECK_EQUAL (2u, model->lambda.size()); - CHECK_EQUAL (2u, model->mu.size()); - CHECK_EQUAL (1u, model->dof_count); + REQUIRE (1u == body_id); + REQUIRE (2u == model->lambda.size()); + REQUIRE (2u == model->mu.size()); + REQUIRE (1u == model->dof_count); - CHECK_EQUAL (2u, model->v.size()); - CHECK_EQUAL (2u, model->a.size()); + REQUIRE (2u == model->v.size()); + REQUIRE (2u == model->a.size()); - CHECK_EQUAL (2u, model->mJoints.size()); - CHECK_EQUAL (2u, model->S.size()); + REQUIRE (2u == model->mJoints.size()); + REQUIRE (2u == model->S.size()); - CHECK_EQUAL (2u, model->c.size()); - CHECK_EQUAL (2u, model->IA.size()); - CHECK_EQUAL (2u, model->pA.size()); - CHECK_EQUAL (2u, model->U.size()); - CHECK_EQUAL (2u, model->d.size()); - CHECK_EQUAL (2u, model->u.size()); - CHECK_EQUAL (2u, model->Ic.size()); - CHECK_EQUAL (2u, model->I.size()); + REQUIRE (2u == model->c.size()); + REQUIRE (2u == model->IA.size()); + REQUIRE (2u == model->pA.size()); + REQUIRE (2u == model->U.size()); + REQUIRE (2u == model->d.size()); + REQUIRE (2u == model->u.size()); + REQUIRE (2u == model->Ic.size()); + REQUIRE (2u == model->I.size()); SpatialVector spatial_zero; spatial_zero.setZero(); - CHECK_EQUAL (2u, model->X_lambda.size()); - CHECK_EQUAL (2u, model->X_base.size()); - CHECK_EQUAL (2u, model->mBodies.size()); + REQUIRE (2u == model->X_lambda.size()); + REQUIRE (2u == model->X_base.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()); - CHECK_EQUAL (3u, model->mu.size()); - CHECK_EQUAL (6u, model->dof_count); - CHECK_EQUAL (7u, model->q_size); - CHECK_EQUAL (6u, model->qdot_size); + REQUIRE (3u == model->lambda.size()); + REQUIRE (3u == model->mu.size()); + REQUIRE (6u == model->dof_count); + REQUIRE (7u == model->q_size); + REQUIRE (6u == model->qdot_size); - CHECK_EQUAL (3u, model->v.size()); - CHECK_EQUAL (3u, model->a.size()); + REQUIRE (3u == model->v.size()); + REQUIRE (3u == model->a.size()); - CHECK_EQUAL (3u, model->mJoints.size()); - CHECK_EQUAL (3u, model->S.size()); + REQUIRE (3u == model->mJoints.size()); + REQUIRE (3u == model->S.size()); - CHECK_EQUAL (3u, model->c.size()); - CHECK_EQUAL (3u, model->IA.size()); - CHECK_EQUAL (3u, model->pA.size()); - CHECK_EQUAL (3u, model->U.size()); - CHECK_EQUAL (3u, model->d.size()); - CHECK_EQUAL (3u, model->u.size()); + REQUIRE (3u == model->c.size()); + REQUIRE (3u == model->IA.size()); + REQUIRE (3u == model->pA.size()); + REQUIRE (3u == model->U.size()); + REQUIRE (3u == model->d.size()); + REQUIRE (3u == model->u.size()); SpatialVector spatial_zero; spatial_zero.setZero(); - CHECK_EQUAL (3u, model->X_lambda.size()); - CHECK_EQUAL (3u, model->X_base.size()); - CHECK_EQUAL (3u, model->mBodies.size()); + REQUIRE (3u == model->X_lambda.size()); + REQUIRE (3u == model->X_base.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); - CHECK_EQUAL (std::numeric_limits::max(), model->GetBodyId("unknownbody")); + REQUIRE (1u == body_id); + REQUIRE (std::numeric_limits::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)); - CHECK_EQUAL (test_joint_axis, model->S[1]); + REQUIRE_THAT (test_vector, AllCloseVector(model->v_J[1], 1.0e-16, 1.0e-16)); + 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)); - CHECK_EQUAL (test_joint_axis, model->S[1]); + REQUIRE_THAT (test_vector, AllCloseVector(model->v_J[1], 1.0e-16, 1.0e-16)); + 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); - CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); + REQUIRE (movable_body + 1 == 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); - CHECK_EQUAL (movable_body, model.lambda[appended_body_id]); - CHECK_EQUAL (movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass); + REQUIRE (movable_body + 1 == appended_body_id); + REQUIRE (movable_body == model.lambda[appended_body_id]); + 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); - CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent); + REQUIRE (movable_body == model.mFixedBodies[fixed_body_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)); - CHECK_EQUAL (string("appended_body"), model.GetBodyName(appended_body_id)); - CHECK_EQUAL (string(""), model.GetBodyName(123)); + REQUIRE (string("fixed_body") == model.GetBodyName(fixed_body_id)); + REQUIRE (string("appended_body") == model.GetBodyName(appended_body_id)); + REQUIRE (string("") == model.GetBodyName(123)); } -TEST_FIXTURE ( RotZRotZYXFixed, ModelGetParentBodyId ) { - CHECK_EQUAL (0u, model->GetParentBodyId(0)); - CHECK_EQUAL (0u, model->GetParentBodyId(body_a_id)); - CHECK_EQUAL (body_a_id, model->GetParentBodyId(body_b_id)); +TEST_CASE_METHOD (RotZRotZYXFixed, __FILE__"_ModelGetParentBodyId", "") { + REQUIRE (0u == model->GetParentBodyId(0)); + REQUIRE (0u == model->GetParentBodyId(body_a_id)); + REQUIRE (body_a_id == model->GetParentBodyId(body_b_id)); } -TEST_FIXTURE(RotZRotZYXFixed, ModelGetParentIdFixed) { - CHECK_EQUAL (body_b_id, model->GetParentBodyId(body_fixed_id)); +TEST_CASE_METHOD(RotZRotZYXFixed, __FILE__"_ModelGetParentIdFixed", "") { + 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); - CHECK_ARRAY_EQUAL (fixture_transform_b.r.data(), transform_b.r.data(), 3); - CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); + REQUIRE_THAT (fixture_transform_a.r, AllCloseVector(transform_a.r, 0., 0.)); + REQUIRE_THAT (fixture_transform_b.r, AllCloseVector(transform_b.r, 0., 0.)); + 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); - CHECK_ARRAY_EQUAL (new_transform_b.r.data(), transform_b.r.data(), 3); - CHECK_ARRAY_EQUAL (Vector3d(0., 0., 0.).data(), transform_root.r.data(), 3); + REQUIRE_THAT (new_transform_a.r, AllCloseVector(transform_a.r, 0., 0.)); + REQUIRE_THAT (new_transform_b.r, AllCloseVector(transform_b.r, 0., 0.)); + 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()); - CHECK_EQUAL ((unsigned int) 2, model.mFixedBodies.size()); + REQUIRE (2 == model.mBodies.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.)); } diff --git a/3rdparty/rbdl/tests/MultiDofTests.cc b/3rdparty/rbdl/tests/MultiDofTests.cc index 5937564..ac95ef1 100644 --- a/3rdparty/rbdl/tests/MultiDofTests.cc +++ b/3rdparty/rbdl/tests/MultiDofTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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) { - CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); - CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); - CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); +TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestQIndices", "") { + REQUIRE(0u == multdof3_model.mJoints[1].q_index); + REQUIRE(1u == multdof3_model.mJoints[2].q_index); + REQUIRE(4u == multdof3_model.mJoints[3].q_index); - CHECK_EQUAL (5u, emulated_model.q_size); - CHECK_EQUAL (5u, emulated_model.qdot_size); + REQUIRE(5u == emulated_model.q_size); + REQUIRE(5u == emulated_model.qdot_size); - CHECK_EQUAL (6u, multdof3_model.q_size); - CHECK_EQUAL (5u, multdof3_model.qdot_size); - CHECK_EQUAL (5u, multdof3_model.multdof3_w_index[2]); + REQUIRE(6u == multdof3_model.q_size); + REQUIRE(5u == multdof3_model.qdot_size); + 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); - CHECK_EQUAL (8u, multdof3_model.qdot_size); + REQUIRE (10u == multdof3_model.q_size); + REQUIRE (8u == multdof3_model.qdot_size); - CHECK_EQUAL (0u, multdof3_model.mJoints[1].q_index); - CHECK_EQUAL (1u, multdof3_model.mJoints[2].q_index); - CHECK_EQUAL (4u, multdof3_model.mJoints[3].q_index); - CHECK_EQUAL (5u, multdof3_model.mJoints[4].q_index); + REQUIRE (0u == multdof3_model.mJoints[1].q_index); + REQUIRE (1u == multdof3_model.mJoints[2].q_index); + REQUIRE (4u == multdof3_model.mJoints[3].q_index); + REQUIRE (5u == multdof3_model.mJoints[4].q_index); - CHECK_EQUAL (8u, multdof3_model.multdof3_w_index[2]); - CHECK_EQUAL (9u, multdof3_model.multdof3_w_index[4]); + REQUIRE (8u == multdof3_model.multdof3_w_index[2]); + 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); - CHECK_ARRAY_CLOSE (emulated_model.a[emu_body_id].data(), multdof3_model.a[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)); + 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); - CHECK_ARRAY_CLOSE (emulated_model.a[emu_child_id].data(), multdof3_model.a[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)); + 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); - CHECK_ARRAY_EQUAL (model_emulated.v[3].data(), model_3dof.v[1].data(), 6); + REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E)); + 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); - CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC); + REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E, TEST_PREC, 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); - CHECK_ARRAY_CLOSE (model_emulated.v[3].data(), model_3dof.v[1].data(), 6, TEST_PREC); + REQUIRE_THAT (model_emulated.X_base[3].E, AllCloseMatrix(model_3dof.X_base[1].E, TEST_PREC, 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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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); - CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyThighRight]].data(), model_3dof->v[body_id_3dof[BodyThighRight]].data(), 6, 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[BodyPelvis]], AllCloseVector(model_3dof->v[body_id_3dof[BodyPelvis]], TEST_PREC, TEST_PREC)); + REQUIRE_THAT (model_emulated->v[body_id_emulated[BodyThighRight]], AllCloseVector(model_3dof->v[body_id_3dof[BodyThighRight]], TEST_PREC, 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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(rand()) / static_cast(RAND_MAX); qdot[i] = 0.4 * M_PI * static_cast(rand()) / static_cast(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(RAND_MAX); tau[i] = rand() / static_cast(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(RAND_MAX); tau[i] = rand() / static_cast(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())); } } diff --git a/3rdparty/rbdl/tests/ScrewJointTests.cc b/3rdparty/rbdl/tests/ScrewJointTests.cc index c5966b8..25d6172 100644 --- a/3rdparty/rbdl/tests/ScrewJointTests.cc +++ b/3rdparty/rbdl/tests/ScrewJointTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include #include @@ -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); - CHECK_ARRAY_EQUAL (Vector3d(1.,0.,0.).data(), model->X_base[roller].r.data(), 3); - CHECK_ARRAY_EQUAL (SpatialVector(0.,0.,2.,cos(q[0])*2,-sin(q[0])*2.,0.).data(), model->v[roller].data(), 6); + REQUIRE_THAT (Xrot(1,Vector3d(0,0,1)).E, AllCloseMatrix(model->X_base[roller].E)); + REQUIRE_THAT (Vector3d(1.,0.,0.), AllCloseVector(model->X_base[roller].r)); + 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]); @@ -82,12 +82,11 @@ 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,13 +98,14 @@ 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); Gexpected(0,0) = 1 - sin(1); Gexpected(1,0) = cos(1); Gexpected(2,0) = 0; - CHECK_ARRAY_EQUAL (Gexpected.data(), GrefPt.data(), 3); + REQUIRE_THAT (Gexpected, AllCloseMatrix(GrefPt)); } diff --git a/3rdparty/rbdl/tests/SparseFactorizationTests.cc b/3rdparty/rbdl/tests/SparseFactorizationTests.cc index 75a60dd..97f478c 100644 --- a/3rdparty/rbdl/tests/SparseFactorizationTests.cc +++ b/3rdparty/rbdl/tests/SparseFactorizationTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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 (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 (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 (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)); } diff --git a/3rdparty/rbdl/tests/SpatialAlgebraTests.cc b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc index 6fed310..3d6e2ef 100644 --- a/3rdparty/rbdl/tests/SpatialAlgebraTests.cc +++ b/3rdparty/rbdl/tests/SpatialAlgebraTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include #include @@ -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); - CHECK_EQUAL (crossf_s_x_t, crossf_s_t); + REQUIRE_THAT (crossm_s_x_t, AllCloseVector(crossm_s_t, 0., 0.)); } -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(), - rbi_added.toMatrix().data(), - 36, - TEST_PREC - ); + + REQUIRE_THAT (rbi_matrix_added, AllCloseMatrix(rbi_added.toMatrix(), TEST_PREC, 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 ( - rbi_matrix_transformed.data(), - rbi_transformed.toMatrix().data(), - 36, - TEST_PREC - ); + REQUIRE_THAT (rbi_matrix_transformed, AllCloseMatrix(rbi_transformed.toMatrix(), TEST_PREC, 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 ( - rbi_matrix_transformed.data(), - rbi_transformed.toMatrix().data(), - 36, - TEST_PREC - ); + REQUIRE_THAT (rbi_matrix_transformed, AllCloseMatrix(rbi_transformed.toMatrix(), TEST_PREC, 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); - CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3); + REQUIRE_THAT (mass, IsClose(rbi.m, 0., 0.)); + 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 diff --git a/3rdparty/rbdl/tests/TwolegModelTests.cc b/3rdparty/rbdl/tests/TwolegModelTests.cc index ca5c379..df467a7 100644 --- a/3rdparty/rbdl/tests/TwolegModelTests.cc +++ b/3rdparty/rbdl/tests/TwolegModelTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_EQUAL (contact_point, heel_point); + REQUIRE (body_id == foot_left_id); + 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; } diff --git a/3rdparty/rbdl/tests/UtilsTests.cc b/3rdparty/rbdl/tests/UtilsTests.cc index 4a97a9d..fcfcf75 100644 --- a/3rdparty/rbdl/tests/UtilsTests.cc +++ b/3rdparty/rbdl/tests/UtilsTests.cc @@ -1,4 +1,4 @@ -#include +#include "rbdl_tests.h" #include @@ -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); - CHECK_EQUAL (Vector3d (0., 0., 0.), com); - CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + REQUIRE (123. == mass); + REQUIRE (Vector3d (0., 0., 0.) == com); + 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); - CHECK_EQUAL (Vector3d (0., 0., 0.), com_velocity); + REQUIRE (Vector3d (0., 1., 0.) == com); + 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); - CHECK_EQUAL (Vector3d (0., 1., 0.), com_velocity); + REQUIRE (Vector3d (0., 1., 0.) == com); + 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); - CHECK (angular_momentum[1] < 0); - CHECK (angular_momentum[2] == 0.); + REQUIRE (angular_momentum[0] == 0); + REQUIRE (angular_momentum[1] < 0); + REQUIRE (angular_momentum[2] == 0.); } template @@ -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( - LinearInvertedPendulumModel, - TestCoMComputationLinearInvertedPendulumModel -) { +TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestCoMComputationLinearInvertedPendulumModel", "") { TestCoMComputation (*this); } -TEST_FIXTURE( - FixedJoint2DoF, - TestCoMComputationFixedJoint2DoF -) { +TEST_CASE_METHOD (FixedJoint2DoF, __FILE__"_TestCoMComputationFixedJoint2DoF", "") { TestCoMComputation (*this); } -TEST_FIXTURE( - FixedBase6DoF12DoFFloatingBase, - TestCoMComputationFixedBase6DoF12DoFFloatingBase -) { +TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_TestCoMComputationFixedBase6DoF12DoFFloatingBase", "") { TestCoMComputation (*this); } -TEST_FIXTURE( - Human36, - TestCoMComputationHuman36 -) { +TEST_CASE_METHOD (Human36, __FILE__"_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); - CHECK_ARRAY_CLOSE (ch_ang_mom_nom.data(), ch_ang_mom_fd.data(), 3, TOL); + REQUIRE_THAT (com_acc_nom, AllCloseVector(com_acc_fd, TOL, TOL)); + REQUIRE_THAT (ch_ang_mom_nom, AllCloseVector(ch_ang_mom_fd, TOL, TOL)); return; } -TEST_FIXTURE( - LinearInvertedPendulumModel, - TestCoMAccelerationUsingFDLinearInvertedPendulumModel -) { +TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestCoMAccelerationUsingFDLinearInvertedPendulumModel", "") { TestCoMAccelerationUsingFD (*this, 1e-8); } -TEST_FIXTURE( - FixedJoint2DoF, - TestCoMAccelerationUsingFDFixedJoint2DoF -) { +TEST_CASE_METHOD (FixedJoint2DoF, __FILE__"_TestCoMAccelerationUsingFDFixedJoint2DoF", "") { TestCoMAccelerationUsingFD (*this, 1e-7); } -TEST_FIXTURE( - FixedBase6DoF12DoFFloatingBase, - TestCoMAccelerationUsingFDFixedBase6DoF12DoFFloatingBase -) { +TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_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( - LinearInvertedPendulumModel, - TestZMPComputationForNotMovingSystemLinearInvertedPendulumModel -) { +TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_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( - LinearInvertedPendulumModel, - TestZMPComputationAgainstTableCartModelLinearInvertedPendulumModel -) { +TEST_CASE_METHOD (LinearInvertedPendulumModel, __FILE__"_TestZMPComputationAgainstTableCartModelLinearInvertedPendulumModel", "") { TestZMPComputationAgainstTableCartModel (*this, 1e-8); } diff --git a/3rdparty/rbdl/tests/main.cc b/3rdparty/rbdl/tests/main.cc index 4608c22..30ad46e 100644 --- a/3rdparty/rbdl/tests/main.cc +++ b/3rdparty/rbdl/tests/main.cc @@ -1,4 +1,6 @@ -#include +#define CATCH_CONFIG_RUNNER +#include "catch.hpp" + #include #include @@ -15,5 +17,5 @@ int main (int argc, char *argv[]) rbdl_print_version(); } - return UnitTest::RunAllTests (); + return Catch::Session().run(argc, argv); } diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e8b079..855c5bb 100644 --- a/CMakeLists.txt +++ b/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