martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Updated RBDL to latest dev commit 73b7048.

master
Martin Felis 2021-10-16 19:35:05 +02:00
parent cc1a106bb9
commit 90d9ac3961
35 changed files with 1435 additions and 1780 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
3rdparty/rbdl/CMakeLists.txt vendored
View File

@ -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 )

7
3rdparty/rbdl/README.md vendored
View File

@ -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
===============

2
3rdparty/rbdl/addons/luamodel/luamodel.cc vendored
View File

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

4
3rdparty/rbdl/include/rbdl/Dynamics.h vendored
View File

@ -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

66
3rdparty/rbdl/include/rbdl/Quaternion.h vendored
View File

@ -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,12 +108,39 @@ class Quaternion : public Vector4d {
}
static Quaternion fromMatrix (const Matrix3d &mat) {
double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
float tr = mat(0,0) + mat(1,1) + mat(2,2);
if (tr > 0) {
float w = sqrt (1.f + tr) * 0.5;
return Quaternion (
(mat(1,2) - mat(2,1)) / (w * 4.),
(mat(2,0) - mat(0,2)) / (w * 4.),
(mat(0,1) - mat(1,0)) / (w * 4.),
w);
} else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
return Quaternion(
x,
(mat(1,0) + mat(0,1)) / (x * 4.),
(mat(2,0) + mat(0,2)) / (x * 4.),
(mat(1,2) - mat(2,1)) / (x * 4.)
);
} else if (mat(1,1) > mat(2,2)) {
float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
return Quaternion(
(mat(1,0) + mat(0,1)) / (y * 4.),
y,
(mat(2,1) + mat(1,2)) / (y * 4.),
(mat(2,0) - mat(0,2)) / (y * 4.)
);
} else {
float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
return Quaternion(
(mat(2,0) + mat(0,2)) / (z * 4.),
(mat(2,1) + mat(1,2)) / (z * 4.),
z,
(mat(0,1) - mat(1,0)) / (z * 4.)
);
}
}
static Quaternion fromZYXAngles (const Vector3d &zyx_angles) {

116
3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h vendored
View File

@ -819,19 +819,16 @@ struct Storage {
inline size_t cols() const { return NumCols; }
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
// Resizing of fixed size matrices not allowed
#ifndef NDEBUG
void resize(int num_rows, int num_cols) {
if (num_rows != NumRows || num_cols != NumCols) {
std::cout << "Error: trying to resize fixed matrix from "
<< NumRows << ", " << NumCols << " to "
<< num_rows << ", " << num_cols << "." << std::endl;
}
assert (num_rows == NumRows && num_cols == NumCols);
#else
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
#endif
// Resizing of fixed size matrices not allowed
assert (num_rows == NumRows && num_cols == NumCols);
}
inline ScalarType& coeff(int row_index, int col_index) {
@ -1555,6 +1552,7 @@ private:
typedef Matrix<value_type, Derived::RowsAtCompileTime, 1> ColumnVector;
bool mIsFactorized;
Matrix<value_type, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> mQ;
Derived mL;
public:
@ -2349,25 +2347,32 @@ class Quaternion : public Vector4f {
Quaternion slerp (float alpha, const Quaternion &quat) const {
// check whether one of the two has 0 length
float s = sqrt (squaredNorm() * quat.squaredNorm());
// 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;
float tr = mat(0,0) + mat(1,1) + mat(2,2);
if (tr > 0) {
float w = sqrt (1.f + tr) * 0.5;
return Quaternion (
(mat(2,1) - mat(1,2)) / (w * 4.f),
(mat(0,2) - mat(2,0)) / (w * 4.f),
(mat(1,0) - mat(0,1)) / (w * 4.f),
(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) {

15
3rdparty/rbdl/src/Joint.cc vendored
View File

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

83
3rdparty/rbdl/tests/BodyTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -14,7 +14,7 @@ const double TEST_PREC = 1.0e-14;
/* Tests whether the spatial inertia matches the one specified by its
* parameters
*/
TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) {
TEST_CASE (__FILE__"_TestComputeSpatialInertiaFromAbsoluteRadiiGyration", "") {
Body body(1.1, Vector3d (1.5, 1.2, 1.3), Vector3d (1.4, 2., 3.));
Matrix3d inertia_C (
@ -35,11 +35,11 @@ TEST ( TestComputeSpatialInertiaFromAbsoluteRadiiGyration ) {
SpatialRigidBodyInertia body_rbi = SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia);
CHECK_ARRAY_CLOSE (reference_inertia.data(), body_rbi.toMatrix().data(), 36, TEST_PREC);
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));
}

13
3rdparty/rbdl/tests/CMakeLists.txt vendored
View File

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

77
3rdparty/rbdl/tests/CalcAccelerationsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -15,7 +15,7 @@ using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-14;
TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) {
TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_TestCalcPointSimple", "") {
QDDot[0] = 1.;
ref_body_id = body_a_id;
point_position = Vector3d (1., 0., 0.);
@ -23,14 +23,12 @@ TEST_FIXTURE(FixedBase3DoF, TestCalcPointSimple) {
// cout << LogOutput.str() << endl;
CHECK_CLOSE(0., point_acceleration[0], TEST_PREC);
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));
}

50
3rdparty/rbdl/tests/CalcVelocitiesTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -58,21 +58,19 @@ struct ModelVelocitiesFixture {
Vector3d point_position, point_velocity;
};
TEST_FIXTURE(ModelVelocitiesFixture, TestCalcPointSimple) {
TEST_CASE_METHOD (ModelVelocitiesFixture, __FILE__"_TestCalcPointSimple", "") {
ref_body_id = 1;
QDot[0] = 1.;
point_position = Vector3d (1., 0., 0.);
point_velocity = CalcPointVelocity(*model, Q, QDot, ref_body_id, point_position);
CHECK_CLOSE(0., point_velocity[0], TEST_PREC);
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));
}

30
3rdparty/rbdl/tests/CompositeRigidBodyTests.cc vendored
View File

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

168
3rdparty/rbdl/tests/ContactsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -103,7 +103,7 @@ struct FixedBase6DoF9DoF {
//
// ForwardDynamicsConstraintsDirect
//
TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectSimple", "") {
Model model;
model.gravity = Vector3d (0., -9.81, 0.);
Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
@ -152,19 +152,14 @@ TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
CHECK_ARRAY_CLOSE (
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;
@ -230,7 +220,7 @@ TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
//
// ForwardDynamicsContacts
//
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContact", "") {
contact_normal.set (0., 1., 0.);
constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal);
ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
@ -255,13 +245,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
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));
}
//
@ -302,7 +292,7 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
// for the example model in FixedBase6DoF and a moving state (i.e. a
// nonzero QDot)
//
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving) {
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContactRotatedMoving", "") {
Q[0] = 0.6;
Q[3] = M_PI * 0.6;
Q[4] = 0.1;
@ -335,14 +325,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving)
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
// check whether FDContactsLagrangian and FDContactsOld match
CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
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
);
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 (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));
}

45
3rdparty/rbdl/tests/CustomConstraintsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include "rbdl/rbdl.h"
#include "rbdl/Constraints.h"
#include <cassert>
@ -335,8 +335,7 @@ struct DoublePerpendicularPendulumCustomConstraint {
TEST(CustomConstraintCorrectnessTest) {
TEST_CASE (__FILE__"_CustomConstraintCorrectnessTest", "") {
//Test to add:
// Jacobian vs. num Jacobian
DoublePerpendicularPendulumCustomConstraint dbcc
@ -430,8 +429,8 @@ TEST(CustomConstraintCorrectnessTest) {
CalcConstraintsVelocityError(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,errd,true);
CHECK(err.norm() >= qError);
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<err.rows();++i){
CHECK_CLOSE(0,err[i],TEST_PREC);
}
for(unsigned int i=0; i<errd.rows();++i){
CHECK_CLOSE(0,errd[i],TEST_PREC);
}
VectorNd target(dbcc.cs.size());
REQUIRE_THAT(target, AllCloseVector(err, TEST_PREC, TEST_PREC));
REQUIRE_THAT(target, AllCloseVector(errd, TEST_PREC, TEST_PREC));
//Evaluate the accelerations of the constrained pendulum and
//compare those to the joint-coordinate pendulum
@ -483,24 +477,16 @@ TEST(CustomConstraintCorrectnessTest) {
ForwardDynamicsConstraintsDirect(dba.model, dba.q, dba.qd,
dba.tau, dba.cs, dba.qdd);
for(unsigned int i = 0; i < dba.cs.G.rows(); ++i){
for(unsigned int j=0; j< dba.cs.G.cols();++j){
CHECK_CLOSE(dba.cs.G(i,j),dbcc.cs.G(i,j),TEST_PREC);
}
}
for(unsigned int i = 0; i < dba.cs.gamma.rows(); ++i){
CHECK_CLOSE(dba.cs.gamma[i],dbcc.cs.gamma[i],TEST_PREC);
}
REQUIRE_THAT (dba.cs.G, AllCloseMatrix(dbcc.cs.G, TEST_PREC, TEST_PREC));
REQUIRE_THAT (dba.cs.gamma, AllCloseVector(dbcc.cs.gamma, TEST_PREC, TEST_PREC));
//REQUIRE_THAT (dba.cs.constraintAxis, AllCloseVector(dbcc.cs.constraintAxis, TEST_PREC, TEST_PREC)); //does not work
for(unsigned int i=0; i < dba.cs.constraintAxis.size(); ++i){
for(unsigned int j=0; j< dba.cs.constraintAxis[0].rows(); ++j){
CHECK_CLOSE(dba.cs.constraintAxis[i][j],
dbcc.cs.constraintAxis[i][j],TEST_PREC);
REQUIRE_THAT (dba.cs.constraintAxis[i][j], IsClose(dbcc.cs.constraintAxis[i][j], TEST_PREC, TEST_PREC));
}
}
SpatialVector a010c =
CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
dbcc.idB1,Vector3d(0.,0.,0.),true);
@ -511,10 +497,7 @@ TEST(CustomConstraintCorrectnessTest) {
CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
dbcc.idB2,Vector3d(dbcc.l2,0.,0.),true);
for(unsigned int i=0; i<6;++i){
CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
}
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
}

177
3rdparty/rbdl/tests/CustomJointMultiBodyTests.cc vendored
View File

@ -4,7 +4,7 @@
*/
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -504,8 +504,7 @@ struct CustomJointMultiBodyFixture {
//
//==============================================================================
TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) {
TEST_CASE_METHOD (CustomJointMultiBodyFixture, __FILE__"_UpdateKinematics", "") {
VectorNd test;
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
@ -535,40 +534,25 @@ TEST_FIXTURE ( CustomJointMultiBodyFixture, UpdateKinematics ) {
// ].E;
// Matrix3d Eerr = Eref-Ecus;
CHECK_ARRAY_CLOSE (
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));
}
}
}
//

142
3rdparty/rbdl/tests/CustomJointSingleBodyTests.cc vendored
View File

@ -4,7 +4,7 @@
*/
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -370,7 +370,7 @@ struct CustomJointSingleBodyFixture {
//
//==============================================================================
TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) {
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_UpdateKinematics", "") {
VectorNd test;
@ -392,27 +392,14 @@ TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) {
qdot.at(idx),
qddot.at(idx));
CHECK_ARRAY_CLOSE (
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,14 +480,7 @@ 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));
}
}
}

2
3rdparty/rbdl/tests/CustomJointTests.cc vendored
View File

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

86
3rdparty/rbdl/tests/DynamicsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
#include <limits>
@ -30,7 +30,7 @@ struct DynamicsFixture {
Model *model;
};
TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) {
TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicSingleChain", "") {
Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
@ -53,10 +53,10 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSingleChain) {
LOG << "a[" << i << "] = " << model->a[i] << endl;
}
CHECK_EQUAL (-4.905, QDDot[0]);
REQUIRE (-4.905 == QDDot[0]);
}
TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) {
TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicSpatialInertiaSingleChain", "") {
// This function checks the value for a non-trivial spatial inertia
Body body(1., Vector3d (1.5, 1., 1.), Vector3d (1., 2., 3.));
@ -82,10 +82,10 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicSpatialInertiaSingleChain) {
LOG << "a[" << i << "] = " << model->a[i] << endl;
}
CHECK_EQUAL (-2.3544, QDDot[0]);
REQUIRE (-2.3544 == QDDot[0]);
}
TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) {
TEST_CASE_METHOD (DynamicsFixture, __FILE__"_TestCalcDynamicDoubleChain", "") {
Body body_a (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
@ -117,11 +117,11 @@ TEST_FIXTURE(DynamicsFixture, TestCalcDynamicDoubleChain) {
// cout << LogOutput.str() << endl;
CHECK_CLOSE (-5.88600000000000E+00, QDDot[0], TEST_PREC);
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<double>(i + 1) * 0.1;
QDot[i] = static_cast<double>(i + 1) * 1.1;
@ -666,10 +666,10 @@ TEST_FIXTURE ( FloatingBase12DoF, TestForwardDynamicsLagrangianPrealloc ) {
&C
);
CHECK_ARRAY_EQUAL (QDDot.data(), QDDot_prealloc.data(), model->dof_count);
REQUIRE_THAT (QDDot, AllCloseVector(QDDot_prealloc));
}
TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) {
TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesTau", "") {
for (unsigned int i = 0; i < model->dof_count; i++) {
Q[i] = rand() / static_cast<double>(RAND_MAX);
Tau[i] = rand() / static_cast<double>(RAND_MAX);
@ -683,10 +683,10 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTau) {
VectorNd qddot_minv (Q);
CalcMInvTimesTau (*model, Q, Tau, qddot_minv);
CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
}
TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) {
TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesTauReuse", "") {
for (unsigned int i = 0; i < model->dof_count; i++) {
Q[i] = rand() / static_cast<double>(RAND_MAX);
Tau[i] = rand() / static_cast<double>(RAND_MAX);
@ -710,11 +710,11 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesTauReuse) {
CalcMInvTimesTau (*model, Q, Tau, qddot_minv, false);
CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
}
}
TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesNonZeroQDotKinematicsUpdate) {
TEST_CASE_METHOD (FixedBase3DoF, __FILE__"_SolveMInvTimesNonZeroQDotKinematicsUpdate", "") {
for (unsigned int i = 0; i < model->dof_count; i++) {
Q[i] = rand() / static_cast<double>(RAND_MAX);
QDot[i] = rand() / static_cast<double>(RAND_MAX);
@ -731,5 +731,5 @@ TEST_FIXTURE ( FixedBase3DoF, SolveMInvTimesNonZeroQDotKinematicsUpdate) {
VectorNd qddot_minv (Q);
CalcMInvTimesTau (*model, Q, Tau, qddot_minv);
CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC);
REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC, TEST_PREC));
}

80
3rdparty/rbdl/tests/FloatingBaseTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -34,7 +34,7 @@ struct FloatingBaseFixture {
VectorNd q, qdot, qddot, tau;
};
TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcPointTransformation", "") {
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
@ -57,10 +57,10 @@ TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
Vector3d test_point;
test_point = CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false);
CHECK_ARRAY_CLOSE (Vector3d (0., -1., 0.).data(), test_point.data(), 3, TEST_PREC);
REQUIRE_THAT (Vector3d (0., -1., 0.), AllCloseVector(test_point, TEST_PREC, TEST_PREC));
}
TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
TEST_CASE_METHOD (FloatingBaseFixture, __FILE__"_TestCalcDynamicFloatingBaseDoubleImplicit", "") {
// floating base
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
@ -88,6 +88,7 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
ForwardDynamics(*model, Q, QDot, Tau, QDDot);
unsigned int i;
for (i = 0; i < QDDot.size(); i++) {
LOG << "QDDot[" << i << "] = " << QDDot[i] << endl;
}
@ -98,13 +99,10 @@ TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
// std::cout << LogOutput.str() << std::endl;
CHECK_CLOSE ( 0.0000, QDDot[0], TEST_PREC);
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));
}

62
3rdparty/rbdl/tests/ForwardDynamicsConstraintsExternalForces.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include "rbdl/rbdl.h"
#include <cassert>
@ -12,18 +12,17 @@ using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-11;
// Reduce an angle to the (-pi, pi] range.
// static double inRange(double angle) {
// 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<err.rows();++i){
CHECK_CLOSE(0,err[i],TEST_PREC);
}
for(unsigned int i=0; i<errd.rows();++i){
CHECK_CLOSE(0,errd[i],TEST_PREC);
}
VectorNd target1 = VectorNd::Zero(err.rows());
REQUIRE_THAT (target1, AllCloseVector(err, TEST_PREC, TEST_PREC));
VectorNd target2 = VectorNd::Zero(errd.rows());
REQUIRE_THAT (target2, AllCloseVector(errd, TEST_PREC, TEST_PREC));
//Evaluate the accelerations of the constrained pendulum and
//compare those to the joint-coordinate pendulum
@ -187,11 +186,9 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
for(unsigned int i=0; i<6;++i){
CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
}
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
ForwardDynamicsConstraintsNullSpace(
dba.model,dba.q,dba.qd,
@ -204,12 +201,9 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
for(unsigned int i=0; i<6;++i){
CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
}
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
ForwardDynamicsConstraintsRangeSpaceSparse(
dba.model,dba.q,dba.qd,
@ -221,11 +215,7 @@ TEST(ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest) {
a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
for(unsigned int i=0; i<6;++i){
CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
}
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
}

28
3rdparty/rbdl/tests/ImpulsesTests.cc vendored
View File

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

6
3rdparty/rbdl/tests/InverseDynamicsTests.cc vendored
View File

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

78
3rdparty/rbdl/tests/InverseKinematicsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -34,7 +34,7 @@ void print_ik_set (const InverseKinematicsConstraintSet &cs) {
}
/// Checks whether a single point in a 3-link chain can be reached
TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) {
TEST_CASE_METHOD (Human36, __FILE__"_ChainSinglePointConstraint", "") {
q[HipRightRY] = 0.3;
q[HipRightRX] = 0.3;
q[HipRightRZ] = 0.3;
@ -59,14 +59,13 @@ TEST_FIXTURE ( Human36, ChainSinglePointConstraint ) {
print_ik_set (cs);
}
CHECK (result);
REQUIRE (result);
CHECK_CLOSE (0., cs.error_norm, TEST_PREC);
REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
}
TEST_FIXTURE ( Human36, ManyPointConstraints ) {
TEST_CASE_METHOD (Human36, __FILE__"_ManyPointConstraints", "") {
randomizeStates();
Vector3d local_point1 (1., 0., 0.);
@ -95,9 +94,9 @@ TEST_FIXTURE ( Human36, ManyPointConstraints ) {
bool result = InverseKinematics (*model, q, cs, qres);
CHECK (result);
REQUIRE (result);
CHECK_CLOSE (0., cs.error_norm, TEST_PREC);
REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
UpdateKinematicsCustom (*model, &qres, NULL, NULL);
Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1);
@ -106,16 +105,16 @@ TEST_FIXTURE ( Human36, ManyPointConstraints ) {
Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4);
Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5);
CHECK_ARRAY_CLOSE (target_position1.data(), result_position1.data(), 3, TEST_PREC);
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));
}

170
3rdparty/rbdl/tests/KinematicsTests.cc vendored
View File

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

650
3rdparty/rbdl/tests/LoopConstraintsTests.cc vendored
View File

File diff suppressed because it is too large Load Diff

76
3rdparty/rbdl/tests/MathTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include "rbdl/Logging.h"
#include "rbdl/rbdl_math.h"
@ -10,10 +10,12 @@ const double TEST_PREC = 1.0e-14;
using namespace std;
using namespace RigidBodyDynamics::Math;
using namespace Catch::Matchers::Floating;
struct MathFixture {
};
TEST (GaussElimPivot) {
TEST_CASE(__FILE__"_GaussElimPivot", "") {
ClearLogOutput();
MatrixNd A;
@ -37,7 +39,7 @@ TEST (GaussElimPivot) {
LinSolveGaussElimPivot (A, b, x);
CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
REQUIRE_THAT (test_result, AllCloseVector(x));
A(0,0) = 0; A(0,1) = -2; A(0,2) = 1;
A(1,0) = 1; A(1,1) = 1; A(1,2) = 5;
@ -48,62 +50,30 @@ TEST (GaussElimPivot) {
test_result[1] = 1;
test_result[2] = 3;
CHECK_ARRAY_CLOSE (test_result.data(), x.data(), 3, TEST_PREC);
x[0] += 1.0e-13;
REQUIRE_THAT (test_result, AllCloseVector(x));
}
TEST (Dynamic_1D_initialize_value) {
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));
}

251
3rdparty/rbdl/tests/ModelTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -28,117 +28,117 @@ struct ModelFixture {
Model *model;
};
TEST_FIXTURE(ModelFixture, TestInit) {
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<unsigned int>::max(), model->GetBodyId("unknownbody"));
REQUIRE (1u == body_id);
REQUIRE (std::numeric_limits<unsigned int>::max() == model->GetBodyId("unknownbody"));
}
TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
TEST_CASE_METHOD(ModelFixture, __FILE__"_TestjcalcSimple", "") {
Body body;
Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
@ -177,8 +177,8 @@ TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
0., 0., 1., 0., 0., 0.
);
CHECK (SpatialVectorCompareEpsilon (test_vector, model->v_J[1], 1.0e-16));
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.));
}

244
3rdparty/rbdl/tests/MultiDofTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -122,7 +122,7 @@ void ConvertQAndQDotFromEmulated (
}
}
TEST(TestQuaternionIntegration ) {
TEST_CASE(__FILE__"_TestQuaternionIntegration", "") {
double timestep = 0.001;
Vector3d zyx_angles_t0 (0.1, 0.2, 0.3);
@ -141,23 +141,23 @@ TEST(TestQuaternionIntegration ) {
// B) integration under the assumption that the angular velocity is
// constant
// However I am not entirely sure about this...
CHECK_ARRAY_CLOSE (q_zyx_t1.data(), q_t1.data(), 4, 1.0e-5);
REQUIRE_THAT(q_zyx_t1, AllCloseVector(q_t1, 1.0e-5, 1.0e-5));
}
TEST_FIXTURE(SphericalJoint, TestQIndices) {
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<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -835,12 +835,12 @@ TEST_FIXTURE (Human36, TestUpdateKinematics) {
UpdateKinematics (*model_emulated, q, qdot, qddot);
UpdateKinematics (*model_3dof, q, qdot, qddot);
CHECK_ARRAY_CLOSE (model_emulated->v[body_id_emulated[BodyPelvis]].data(), model_3dof->v[body_id_3dof[BodyPelvis]].data(), 6, TEST_PREC);
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<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -855,10 +855,10 @@ TEST_FIXTURE (Human36, TestInverseDynamics) {
InverseDynamics (*model_emulated, q, qdot, qddot, id_emulated);
InverseDynamics (*model_3dof, q, qdot, qddot, id_3dof);
CHECK_ARRAY_CLOSE (id_emulated.data(), id_3dof.data(), id_emulated.size(), TEST_PREC * id_emulated.norm());
REQUIRE_THAT (id_emulated, AllCloseVector(id_3dof, TEST_PREC * id_emulated.norm(), TEST_PREC * id_emulated.norm()));
}
TEST_FIXTURE (Human36, TestNonlinearEffects) {
TEST_CASE_METHOD (Human36, __FILE__"_TestNonlinearEffects", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -873,10 +873,10 @@ TEST_FIXTURE (Human36, TestNonlinearEffects) {
NonlinearEffects (*model_emulated, q, qdot, nle_emulated);
NonlinearEffects (*model_3dof, q, qdot, nle_3dof);
CHECK_ARRAY_CLOSE (nle_emulated.data(), nle_3dof.data(), nle_emulated.size(), TEST_PREC * nle_emulated.norm());
REQUIRE_THAT (nle_emulated, AllCloseVector(nle_3dof, TEST_PREC * nle_emulated.norm(), TEST_PREC * nle_emulated.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianKokkevis", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -888,19 +888,19 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianKokkevis) {
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
ForwardDynamicsContactsKokkevis (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_kokkevis.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianSparse", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -912,18 +912,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianSparse) {
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_sparse);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_sparse);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_sparse);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedLagrangianNullSpace", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -935,18 +935,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedLagrangianNullSpace) {
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_nullspace);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_nullspace);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_lagrangian);
ForwardDynamicsConstraintsNullSpace (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_nullspace);
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_nullspace.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm());
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_nullspace, TEST_PREC * qddot_lagrangian.norm(), TEST_PREC * qddot_lagrangian.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofLagrangian", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -956,18 +956,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofLagrangian) {
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
ForwardDynamicsConstraintsDirect (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
ForwardDynamicsConstraintsDirect (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofSparse", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -977,22 +977,22 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofSparse) {
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
for (unsigned int i = 0; i < q.size(); i++) {
CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
REQUIRE (model_emulated->lambda_q[i]==model_3dof->lambda_q[i]);
}
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B4C_emulated, qddot_emulated);
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_4B4C_emulated, qddot_emulated);
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_3dof);
CHECK_ARRAY_CLOSE (qddot_emulated.data(), qddot_3dof.data(), qddot_emulated.size(), TEST_PREC * qddot_emulated.norm());
REQUIRE_THAT (qddot_emulated, AllCloseVector(qddot_3dof, TEST_PREC * qddot_emulated.norm(), TEST_PREC * qddot_emulated.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofKokkevisSparse", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -1002,7 +1002,7 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
ForwardDynamicsConstraintsRangeSpaceSparse (*model_emulated, q, qdot, tau, constraints_1B1C_emulated, qddot_emulated);
for (unsigned int i = 0; i < q.size(); i++) {
CHECK_EQUAL (model_emulated->lambda_q[i], model_3dof->lambda_q[i]);
REQUIRE (model_emulated->lambda_q[i]==model_3dof->lambda_q[i]);
}
VectorNd qddot_sparse (qddot_emulated);
@ -1010,18 +1010,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisSparse) {
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_sparse);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_sparse);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_sparse);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
CHECK_ARRAY_CLOSE (qddot_sparse.data(), qddot_kokkevis.data(), qddot_sparse.size(), TEST_PREC * qddot_sparse.norm());
REQUIRE_THAT (qddot_sparse, AllCloseVector(qddot_kokkevis, TEST_PREC * qddot_sparse.norm(), TEST_PREC * qddot_sparse.norm()));
}
TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) {
TEST_CASE_METHOD (Human36, __FILE__"_TestContactsEmulatedMultdofKokkevisMultiple", "") {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
@ -1033,18 +1033,18 @@ TEST_FIXTURE (Human36, TestContactsEmulatedMultdofKokkevisMultiple ) {
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B1C_3dof, qddot_kokkevis_2);
CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_1B4C_3dof, qddot_kokkevis_2);
CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis);
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraints_4B4C_3dof, qddot_kokkevis_2);
CHECK_ARRAY_CLOSE (qddot_kokkevis.data(), qddot_kokkevis_2.data(), qddot_kokkevis.size(), TEST_PREC * qddot_kokkevis.norm());
REQUIRE_THAT (qddot_kokkevis, AllCloseVector(qddot_kokkevis_2, TEST_PREC * qddot_kokkevis.norm(), TEST_PREC * qddot_kokkevis.norm()));
}
TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) {
TEST_CASE_METHOD(SphericalJoint, __FILE__"_TestEulerZYXvsEmulated", "") {
emuQ[0] = 1.1;
emuQ[1] = 1.2;
emuQ[2] = 1.3;
@ -1069,10 +1069,10 @@ TEST_FIXTURE(SphericalJoint, TestEulerZYXvsEmulated ) {
ForwardDynamicsLagrangian (emulated_model, emuQ, emuQDot, emuTau, QDDot_emu);
ForwardDynamicsLagrangian (eulerzyx_model, emuQ, emuQDot, emuTau, QDDot_eulerzyx);
CHECK_ARRAY_CLOSE (QDDot_emu.data(), QDDot_eulerzyx.data(), emulated_model.qdot_size, TEST_PREC);
REQUIRE_THAT (QDDot_emu, AllCloseVector(QDDot_eulerzyx, TEST_PREC, TEST_PREC));
}
TEST_FIXTURE ( Human36, SolveMInvTimesTau) {
TEST_CASE_METHOD (Human36, __FILE__"_SolveMInvTimesTau", "") {
for (unsigned int i = 0; i < model->dof_count; i++) {
q[i] = rand() / static_cast<double>(RAND_MAX);
tau[i] = rand() / static_cast<double>(RAND_MAX);
@ -1086,10 +1086,10 @@ TEST_FIXTURE ( Human36, SolveMInvTimesTau) {
VectorNd qddot_minv (q);
CalcMInvTimesTau (*model, q, tau, qddot_minv);
CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_minv.norm());
REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC * qddot_minv.norm(), TEST_PREC * qddot_minv.norm()));
}
TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) {
TEST_CASE_METHOD (Human36, __FILE__"_SolveMInvTimesTauReuse", "") {
for (unsigned int i = 0; i < model->dof_count; i++) {
q[i] = rand() / static_cast<double>(RAND_MAX);
tau[i] = rand() / static_cast<double>(RAND_MAX);
@ -1113,6 +1113,6 @@ TEST_FIXTURE ( Human36, SolveMInvTimesTauReuse) {
CalcMInvTimesTau (*model, q, tau, qddot_minv);
CHECK_ARRAY_CLOSE (qddot_solve_llt.data(), qddot_minv.data(), model->dof_count, TEST_PREC * qddot_solve_llt.norm());
REQUIRE_THAT (qddot_solve_llt, AllCloseVector(qddot_minv, TEST_PREC * qddot_solve_llt.norm(), TEST_PREC * qddot_solve_llt.norm()));
}
}

20
3rdparty/rbdl/tests/ScrewJointTests.cc vendored
View File

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

34
3rdparty/rbdl/tests/SparseFactorizationTests.cc vendored
View File

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

130
3rdparty/rbdl/tests/SpatialAlgebraTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
#include <iomanip>
@ -38,7 +38,7 @@ Vector3d get_translation (const SpatialMatrix &m) {
}
/// \brief Checks the multiplication of a SpatialMatrix with a SpatialVector
TEST(TestSpatialMatrixTimesSpatialVector) {
TEST_CASE(__FILE__"_TestSpatialMatrixTimesSpatialVector", "") {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
@ -57,11 +57,12 @@ TEST(TestSpatialMatrixTimesSpatialVector) {
SpatialVector test_result (
43., 44., 45., 34., 35., 40.
);
CHECK_EQUAL (test_result, result);
REQUIRE_THAT(test_result, AllCloseVector(result, 0., 0.));
}
/// \brief Checks the multiplication of a scalar with a SpatialVector
TEST(TestScalarTimesSpatialVector) {
TEST_CASE(__FILE__"_TestScalarTimesSpatialVector", "") {
SpatialVector s_vector (
1., 2., 3., 4., 5., 6.
);
@ -71,11 +72,12 @@ TEST(TestScalarTimesSpatialVector) {
SpatialVector test_result(3., 6., 9., 12., 15., 18.);
CHECK_EQUAL (test_result, result);
REQUIRE_THAT (test_result, AllCloseVector(result, 0., 0.));
}
/// \brief Checks the multiplication of a scalar with a SpatialMatrix
TEST(TestScalarTimesSpatialMatrix) {
TEST_CASE(__FILE__"_TestScalarTimesSpatialMatrix", "") {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
@ -97,11 +99,12 @@ TEST(TestScalarTimesSpatialMatrix) {
12., 0., 0., 0., 0., 18.
);
CHECK_EQUAL (test_result, result);
REQUIRE_THAT (test_result, AllCloseMatrix(result, 0., 0.));
}
/// \brief Checks the multiplication of a scalar with a SpatialMatrix
TEST(TestSpatialMatrixTimesSpatialMatrix) {
TEST_CASE(__FILE__"_TestSpatialMatrixTimesSpatialMatrix", "") {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
@ -123,14 +126,14 @@ TEST(TestSpatialMatrixTimesSpatialMatrix) {
28., 0., 0., 0., 0., 64.
);
CHECK_EQUAL (test_result, result);
REQUIRE_THAT(test_result, AllCloseMatrix(result, 0., 0.));
}
/// \brief Checks the adjoint method
//
// This method computes a spatial force transformation from a spatial
// motion transformation and vice versa
TEST(TestSpatialMatrixTransformAdjoint) {
TEST_CASE(__FILE__"_TestSpatialMatrixTransformAdjoint", "") {
SpatialMatrix s_matrix (
1., 2., 3., 4., 5., 6.,
7., 8., 9., 10., 11., 12.,
@ -150,10 +153,10 @@ TEST(TestSpatialMatrixTransformAdjoint) {
10., 11., 12., 28., 29., 30.,
16., 17., 18., 34., 35., 36.);
CHECK_EQUAL (test_result_matrix, result);
REQUIRE_THAT (test_result_matrix, AllCloseMatrix(result, 0., 0.));
}
TEST(TestSpatialMatrixInverse) {
TEST_CASE(__FILE__"_TestSpatialMatrixInverse", "") {
SpatialMatrix s_matrix (
0, 1, 2, 0, 1, 2,
3, 4, 5, 3, 4, 5,
@ -172,10 +175,10 @@ TEST(TestSpatialMatrixInverse) {
2, 5, 8, 2, 5, 8
);
CHECK_EQUAL (test_inv, spatial_inverse(s_matrix));
REQUIRE_THAT (test_inv, AllCloseMatrix(spatial_inverse(s_matrix), 0., 0.));
}
TEST(TestSpatialMatrixGetRotation) {
TEST_CASE(__FILE__"_TestSpatialMatrixGetRotation", "") {
SpatialMatrix spatial_transform (
1., 2., 3., 0., 0., 0.,
4., 5., 6., 0., 0., 0.,
@ -193,10 +196,10 @@ TEST(TestSpatialMatrixGetRotation) {
7., 8., 9.
);
CHECK_EQUAL(test_result, rotation);
REQUIRE_THAT (test_result, AllCloseMatrix(rotation, 0., 0.));
}
TEST(TestSpatialMatrixGetTranslation) {
TEST_CASE(__FILE__"_TestSpatialMatrixGetTranslation", "") {
SpatialMatrix spatial_transform (
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
@ -211,10 +214,10 @@ TEST(TestSpatialMatrixGetTranslation) {
1., 2., 3.
);
CHECK_EQUAL( test_result, translation);
REQUIRE_THAT (test_result, AllCloseMatrix(translation, 0., 0.));
}
TEST(TestSpatialVectorCross) {
TEST_CASE(__FILE__"_TestSpatialVectorCross", "") {
SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
SpatialMatrix test_cross (
@ -227,15 +230,15 @@ TEST(TestSpatialVectorCross) {
);
SpatialMatrix s_vec_cross (crossm(s_vec));
CHECK_EQUAL (test_cross, s_vec_cross);
REQUIRE_THAT (test_cross, AllCloseMatrix(s_vec_cross, 0., 0.));
SpatialMatrix s_vec_crossf (crossf(s_vec));
SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose();
CHECK_EQUAL (test_crossf, s_vec_crossf);
REQUIRE_THAT (test_crossf, AllCloseMatrix(s_vec_crossf, 0., 0.));
}
TEST(TestSpatialVectorCrossmCrossf) {
TEST_CASE(__FILE__"_TestSpatialVectorCrossmCrossf", "") {
SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
SpatialVector t_vec (9., 8., 7., 6., 5., 4.);
@ -255,11 +258,10 @@ TEST(TestSpatialVectorCrossmCrossf) {
cout << crossf_s_t << endl;
*/
CHECK_EQUAL (crossm_s_x_t, crossm_s_t);
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

16
3rdparty/rbdl/tests/TwolegModelTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -181,7 +181,7 @@ Vector3d heel_point (0., 0., 0.);
Vector3d medial_point (0., 0., 0.);
void init_model (Model* model) {
assert (model);
REQUIRE (model);
constraint_set_right = ConstraintSet();
constraint_set_left = ConstraintSet();
@ -272,7 +272,7 @@ void copy_values (T *dest, const T *src, size_t count) {
memcpy (dest, src, count * sizeof (T));
}
TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) {
TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectFootmodel", "" ) {
Model* model = new Model;
init_model(model);
@ -335,8 +335,8 @@ TEST ( TestForwardDynamicsConstraintsDirectFootmodel ) {
contact_force[0] = constraint_set_left.force[0];
contact_force[1] = constraint_set_left.force[1];
CHECK_EQUAL (body_id, foot_left_id);
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;
}

103
3rdparty/rbdl/tests/UtilsTests.cc vendored
View File

@ -1,4 +1,4 @@
#include <UnitTest++.h>
#include "rbdl_tests.h"
#include <iostream>
@ -16,7 +16,7 @@ using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) {
TEST_CASE_METHOD (FloatingBase12DoF, __FILE__"_TestKineticEnergy", "") {
VectorNd q = VectorNd::Zero(model->q_size);
VectorNd qdot = VectorNd::Zero(model->q_size);
@ -31,10 +31,10 @@ TEST_FIXTURE(FloatingBase12DoF, TestKineticEnergy) {
double kinetic_energy_ref = 0.5 * qdot.transpose() * H * qdot;
double kinetic_energy = Utils::CalcKineticEnergy (*model, q, qdot);
CHECK_EQUAL (kinetic_energy_ref, kinetic_energy);
REQUIRE (kinetic_energy_ref == kinetic_energy);
}
TEST(TestPotentialEnergy) {
TEST_CASE (__FILE__"_TestPotentialEnergy", "") {
Model model;
Matrix3d inertia = Matrix3d::Zero(3,3);
Body body (0.5, Vector3d (0., 0., 0.), inertia);
@ -48,14 +48,14 @@ TEST(TestPotentialEnergy) {
VectorNd q = VectorNd::Zero(model.q_size);
double potential_energy_zero = Utils::CalcPotentialEnergy (model, q);
CHECK_EQUAL (0., potential_energy_zero);
REQUIRE (0. == potential_energy_zero);
q[1] = 1.;
double potential_energy_lifted = Utils::CalcPotentialEnergy (model, q);
CHECK_EQUAL (4.905, potential_energy_lifted);
REQUIRE (4.905 == potential_energy_lifted);
}
TEST(TestCOMSimple) {
TEST_CASE (__FILE__"_TestCOMSimple", "") {
Model model;
Matrix3d inertia = Matrix3d::Zero(3,3);
Body body (123., Vector3d (0., 0., 0.), inertia);
@ -75,22 +75,22 @@ TEST(TestCOMSimple) {
Vector3d com_velocity;
Utils::CalcCenterOfMass (model, q, qdot, NULL, mass, com, &com_velocity);
CHECK_EQUAL (123., mass);
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 <typename T>
@ -183,36 +183,24 @@ void TestCoMComputation (
NULL, NULL
);
CHECK_CLOSE (mass_expected, mass_actual, 1e-7);
REQUIRE_THAT (mass_expected, IsClose(mass_actual, 1e-7, 1e-7));
return;
}
TEST_FIXTURE(
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);
}

6
3rdparty/rbdl/tests/main.cc vendored
View File

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

2
CMakeLists.txt
View File

@ -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