Updated RBDL to latest dev commit 73b7048.
Martin Felis
parent
cc1a106bb9
commit
90d9ac3961
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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
|
||||
===============
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@ extern "C"
|
|||
#include <lualib.h>
|
||||
}
|
||||
|
||||
|
||||
|
||||
using namespace std;
|
||||
using namespace RigidBodyDynamics;
|
||||
using namespace RigidBodyDynamics::Math;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ struct Model;
|
|||
*
|
||||
* This function computes the generalized forces from given generalized
|
||||
* states, velocities, and accelerations:
|
||||
* \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$
|
||||
* \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}, f_\textit{ext}) \f$
|
||||
*
|
||||
* \param model rigid body model
|
||||
* \param Q state vector of the internal joints
|
||||
|
|
@ -55,7 +55,7 @@ RBDL_DLLAPI void InverseDynamics (
|
|||
*
|
||||
* This function computes the generalized forces from given generalized
|
||||
* states, velocities, and accelerations:
|
||||
* \f$ \tau = M(q) \ddot{q} + N(q, \dot{q}) \f$
|
||||
* \f$ \tau = N(q, \dot{q}, f_\textit{ext}) \f$
|
||||
*
|
||||
* \param model rigid body model
|
||||
* \param Q state vector of the internal joints
|
||||
|
|
@ -144,7 +144,7 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian (
|
|||
Math::LinearSolver linear_solver = Math::LinearSolverColPivHouseholderQR,
|
||||
std::vector<Math::SpatialVector> *f_ext = NULL,
|
||||
Math::MatrixNd *H = NULL,
|
||||
Math::VectorNd *C = NULL
|
||||
Math::VectorNd *C = NULL
|
||||
);
|
||||
|
||||
/** \brief Computes the effect of multiplying the inverse of the joint
|
||||
|
|
@ -158,7 +158,7 @@ RBDL_DLLAPI void ForwardDynamicsLagrangian (
|
|||
* \param update_kinematics whether the kinematics should be updated (safer, but at a higher computational cost)
|
||||
*
|
||||
* This function uses a reduced version of the Articulated %Body Algorithm
|
||||
* to compute:
|
||||
* to compute:
|
||||
*
|
||||
* \f$ \ddot{q} = M(q)^{-1} \tau\f$
|
||||
*
|
||||
|
|
|
|||
|
|
@ -66,27 +66,34 @@ class Quaternion : public Vector4d {
|
|||
);
|
||||
}
|
||||
|
||||
Quaternion slerp (double alpha, const Quaternion &quat) const {
|
||||
Quaternion slerp (double alpha, Quaternion quat) const {
|
||||
// check whether one of the two has 0 length
|
||||
double s = std::sqrt (squaredNorm() * quat.squaredNorm());
|
||||
|
||||
// division by 0.f is unhealthy!
|
||||
assert (s != 0.);
|
||||
|
||||
double angle = acos (dot(quat) / s);
|
||||
if (angle == 0. || std::isnan(angle)) {
|
||||
double cos_half_theta = this->dot(quat);
|
||||
if (fabs(cos_half_theta >= 1.0)) {
|
||||
return *this;
|
||||
}
|
||||
assert(!std::isnan(angle));
|
||||
|
||||
double d = 1. / std::sin (angle);
|
||||
double p0 = std::sin ((1. - alpha) * angle);
|
||||
double p1 = std::sin (alpha * angle);
|
||||
double half_theta = acos(cos_half_theta);
|
||||
double sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta);
|
||||
|
||||
if (dot (quat) < 0.) {
|
||||
return Quaternion( ((*this) * p0 - quat * p1) * d);
|
||||
if (fabs(sin_half_theta) < 0.00001) {
|
||||
return Quaternion (
|
||||
((*this)[0] * 0.5 + quat[0] * 0.5),
|
||||
((*this)[1] * 0.5 + quat[1] * 0.5),
|
||||
((*this)[2] * 0.5 + quat[2] * 0.5),
|
||||
((*this)[3] * 0.5 + quat[3] * 0.5)
|
||||
);
|
||||
}
|
||||
return Quaternion( ((*this) * p0 + quat * p1) * d);
|
||||
|
||||
double ratio_a = sin((1 - alpha) * half_theta) / sin_half_theta;
|
||||
double ratio_b = sin(alpha * half_theta) / sin_half_theta;
|
||||
|
||||
return Quaternion (
|
||||
((*this)[0] * ratio_a + quat[0] * ratio_b),
|
||||
((*this)[1] * ratio_a + quat[1] * ratio_b),
|
||||
((*this)[2] * ratio_a + quat[2] * ratio_b),
|
||||
((*this)[3] * ratio_a + quat[3] * ratio_b)
|
||||
);
|
||||
}
|
||||
|
||||
static Quaternion fromAxisAngle (const Vector3d &axis, double angle_rad) {
|
||||
|
|
@ -101,18 +108,45 @@ class Quaternion : public Vector4d {
|
|||
}
|
||||
|
||||
static Quaternion fromMatrix (const Matrix3d &mat) {
|
||||
double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
|
||||
return Quaternion (
|
||||
(mat(1,2) - mat(2,1)) / (w * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (w * 4.),
|
||||
(mat(0,1) - mat(1,0)) / (w * 4.),
|
||||
w);
|
||||
float tr = mat(0,0) + mat(1,1) + mat(2,2);
|
||||
if (tr > 0) {
|
||||
float w = sqrt (1.f + tr) * 0.5;
|
||||
return Quaternion (
|
||||
(mat(1,2) - mat(2,1)) / (w * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (w * 4.),
|
||||
(mat(0,1) - mat(1,0)) / (w * 4.),
|
||||
w);
|
||||
} else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
|
||||
float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
x,
|
||||
(mat(1,0) + mat(0,1)) / (x * 4.),
|
||||
(mat(2,0) + mat(0,2)) / (x * 4.),
|
||||
(mat(1,2) - mat(2,1)) / (x * 4.)
|
||||
);
|
||||
} else if (mat(1,1) > mat(2,2)) {
|
||||
float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(1,0) + mat(0,1)) / (y * 4.),
|
||||
y,
|
||||
(mat(2,1) + mat(1,2)) / (y * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (y * 4.)
|
||||
);
|
||||
} else {
|
||||
float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(2,0) + mat(0,2)) / (z * 4.),
|
||||
(mat(2,1) + mat(1,2)) / (z * 4.),
|
||||
z,
|
||||
(mat(0,1) - mat(1,0)) / (z * 4.)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
static Quaternion fromZYXAngles (const Vector3d &zyx_angles) {
|
||||
return Quaternion::fromAxisAngle (Vector3d (0., 0., 1.), zyx_angles[0])
|
||||
* Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), zyx_angles[1])
|
||||
* Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]);
|
||||
* Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), zyx_angles[2]);
|
||||
}
|
||||
|
||||
static Quaternion fromYXZAngles (const Vector3d &yxz_angles) {
|
||||
|
|
@ -122,7 +156,7 @@ class Quaternion : public Vector4d {
|
|||
}
|
||||
|
||||
static Quaternion fromXYZAngles (const Vector3d &xyz_angles) {
|
||||
return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2])
|
||||
return Quaternion::fromAxisAngle (Vector3d (0., 0., 01.), xyz_angles[2])
|
||||
* Quaternion::fromAxisAngle (Vector3d (0., 1., 0.), xyz_angles[1])
|
||||
* Quaternion::fromAxisAngle (Vector3d (1., 0., 0.), xyz_angles[0]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -819,19 +819,16 @@ struct Storage {
|
|||
|
||||
inline size_t cols() const { return NumCols; }
|
||||
|
||||
#ifndef NDEBUG
|
||||
void resize(int num_rows, int num_cols) {
|
||||
if (num_rows != NumRows || num_cols != NumCols) {
|
||||
std::cout << "Error: trying to resize fixed matrix from "
|
||||
<< NumRows << ", " << NumCols << " to "
|
||||
<< num_rows << ", " << num_cols << "." << std::endl;
|
||||
}
|
||||
assert (num_rows == NumRows && num_cols == NumCols);
|
||||
#else
|
||||
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
|
||||
#endif
|
||||
// Resizing of fixed size matrices not allowed
|
||||
|
||||
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
|
||||
// Resizing of fixed size matrices not allowed
|
||||
#ifndef NDEBUG
|
||||
if (num_rows != NumRows || num_cols != NumCols) {
|
||||
std::cout << "Error: trying to resize fixed matrix from "
|
||||
<< NumRows << ", " << NumCols << " to "
|
||||
<< num_rows << ", " << num_cols << "." << std::endl;
|
||||
}
|
||||
#endif
|
||||
assert (num_rows == NumRows && num_cols == NumCols);
|
||||
}
|
||||
|
||||
inline ScalarType& coeff(int row_index, int col_index) {
|
||||
|
|
@ -1555,6 +1552,7 @@ private:
|
|||
typedef Matrix<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;
|
||||
return Quaternion (
|
||||
(mat(2,1) - mat(1,2)) / (w * 4.f),
|
||||
(mat(0,2) - mat(2,0)) / (w * 4.f),
|
||||
(mat(1,0) - mat(0,1)) / (w * 4.f),
|
||||
w);
|
||||
float tr = mat(0,0) + mat(1,1) + mat(2,2);
|
||||
if (tr > 0) {
|
||||
float w = sqrt (1.f + tr) * 0.5;
|
||||
return Quaternion (
|
||||
(mat(1,2) - mat(2,1)) / (w * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (w * 4.),
|
||||
(mat(0,1) - mat(1,0)) / (w * 4.),
|
||||
w);
|
||||
} else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
|
||||
float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
x,
|
||||
(mat(1,0) + mat(0,1)) / (x * 4.),
|
||||
(mat(2,0) + mat(0,2)) / (x * 4.),
|
||||
(mat(1,2) - mat(2,1)) / (x * 4.)
|
||||
);
|
||||
} else if (mat(1,1) > mat(2,2)) {
|
||||
float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(1,0) + mat(0,1)) / (y * 4.),
|
||||
y,
|
||||
(mat(2,1) + mat(1,2)) / (y * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (y * 4.)
|
||||
);
|
||||
} else {
|
||||
float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(2,0) + mat(0,2)) / (z * 4.),
|
||||
(mat(2,1) + mat(1,2)) / (z * 4.),
|
||||
z,
|
||||
(mat(0,1) - mat(1,0)) / (z * 4.)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
static Quaternion fromMatrixN (const Matrix33f &mat) {
|
||||
float tr = mat(0,0) + mat(1,1) + mat(2,2);
|
||||
if (tr > 0) {
|
||||
float w = sqrt (1.f + tr) * 0.5;
|
||||
return Quaternion (
|
||||
(mat(1,2) - mat(2,1)) / (w * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (w * 4.),
|
||||
(mat(0,1) - mat(1,0)) / (w * 4.),
|
||||
w);
|
||||
} else if ((mat(0,0) > mat(1,1)) && (mat(0,0) > mat(2,2))) {
|
||||
float x = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
x,
|
||||
(mat(1,0) + mat(0,1)) / (x * 4.),
|
||||
(mat(2,0) + mat(0,2)) / (x * 4.),
|
||||
(mat(1,2) - mat(2,1)) / (x * 4.)
|
||||
);
|
||||
} else if (mat(1,1) > mat(2,2)) {
|
||||
float y = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(1,0) + mat(0,1)) / (y * 4.),
|
||||
y,
|
||||
(mat(2,1) + mat(1,2)) / (y * 4.),
|
||||
(mat(2,0) - mat(0,2)) / (y * 4.)
|
||||
);
|
||||
} else {
|
||||
float z = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 0.5;
|
||||
return Quaternion(
|
||||
(mat(2,0) + mat(0,2)) / (z * 4.),
|
||||
(mat(2,1) + mat(1,2)) / (z * 4.),
|
||||
z,
|
||||
(mat(0,1) - mat(1,0)) / (z * 4.)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -92,21 +107,21 @@ RBDL_DLLAPI void jcalc (
|
|||
jcalc_X_lambda_S(model, joint_id, q);
|
||||
double Jqd = qdot[model.mJoints[joint_id].q_index];
|
||||
model.v_J[joint_id] = model.S[joint_id] * Jqd;
|
||||
|
||||
|
||||
Vector3d St = model.S[joint_id].block(0,0,3,1);
|
||||
Vector3d c = X_J.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
|
||||
c = St.cross(c);
|
||||
c *= -Jqd * Jqd;
|
||||
c *= -Jqd * Jqd;
|
||||
model.c_J[joint_id] = SpatialVector(0,0,0,c[0],c[1],c[2]);
|
||||
model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
|
||||
} else if (model.mJoints[joint_id].mDoFCount == 1 &&
|
||||
model.mJoints[joint_id].mJointType != JointTypeCustom) {
|
||||
SpatialTransform X_J = jcalc_XJ (model, joint_id, q);
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.S[joint_id] * qdot[model.mJoints[joint_id].q_index];
|
||||
model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
|
||||
} else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
|
||||
SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(),
|
||||
SpatialTransform X_J = SpatialTransform (model.GetQuaternion (joint_id, q).toMatrix(),
|
||||
Vector3d (0., 0., 0.));
|
||||
|
||||
model.multdof3_S[joint_id](0,0) = 1.;
|
||||
|
|
@ -153,7 +168,7 @@ RBDL_DLLAPI void jcalc (
|
|||
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
|
||||
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
|
||||
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
|
||||
|
||||
model.c_J[joint_id].set(
|
||||
|
|
@ -194,7 +209,7 @@ RBDL_DLLAPI void jcalc (
|
|||
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
|
||||
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
|
||||
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
|
||||
|
||||
model.c_J[joint_id].set(
|
||||
|
|
@ -235,7 +250,7 @@ RBDL_DLLAPI void jcalc (
|
|||
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
|
||||
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
|
||||
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
|
||||
|
||||
model.c_J[joint_id].set(
|
||||
|
|
@ -279,7 +294,7 @@ RBDL_DLLAPI void jcalc (
|
|||
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
|
||||
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
|
||||
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
|
||||
|
||||
model.c_J[joint_id].set(
|
||||
|
|
@ -301,7 +316,7 @@ RBDL_DLLAPI void jcalc (
|
|||
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
|
||||
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
|
||||
|
||||
model.v_J[joint_id] =
|
||||
model.v_J[joint_id] =
|
||||
model.multdof3_S[joint_id] * Vector3d (qdot0, qdot1, qdot2);
|
||||
|
||||
model.c_J[joint_id].set(0., 0., 0., 0., 0., 0.);
|
||||
|
|
@ -309,7 +324,7 @@ RBDL_DLLAPI void jcalc (
|
|||
model.X_lambda[joint_id].r = model.X_T[joint_id].r + model.X_T[joint_id].E.transpose() * Vector3d (q0, q1, q2);
|
||||
} else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
|
||||
const Joint &joint = model.mJoints[joint_id];
|
||||
CustomJoint *custom_joint =
|
||||
CustomJoint *custom_joint =
|
||||
model.mCustomJoints[joint.custom_joint_index];
|
||||
custom_joint->jcalc (model, joint_id, q, qdot);
|
||||
} else {
|
||||
|
|
@ -337,9 +352,9 @@ RBDL_DLLAPI Math::SpatialTransform jcalc_XJ (
|
|||
return Xtrans ( Vector3d (
|
||||
model.mJoints[joint_id].mJointAxes[0][3]
|
||||
* q[model.mJoints[joint_id].q_index],
|
||||
model.mJoints[joint_id].mJointAxes[0][4]
|
||||
model.mJoints[joint_id].mJointAxes[0][4]
|
||||
* q[model.mJoints[joint_id].q_index],
|
||||
model.mJoints[joint_id].mJointAxes[0][5]
|
||||
model.mJoints[joint_id].mJointAxes[0][5]
|
||||
* q[model.mJoints[joint_id].q_index]
|
||||
)
|
||||
);
|
||||
|
|
@ -353,9 +368,9 @@ RBDL_DLLAPI Math::SpatialTransform jcalc_XJ (
|
|||
SpatialTransform trans = Xtrans ( Vector3d (
|
||||
model.mJoints[joint_id].mJointAxes[0][3]
|
||||
* q[model.mJoints[joint_id].q_index],
|
||||
model.mJoints[joint_id].mJointAxes[0][4]
|
||||
model.mJoints[joint_id].mJointAxes[0][4]
|
||||
* q[model.mJoints[joint_id].q_index],
|
||||
model.mJoints[joint_id].mJointAxes[0][5]
|
||||
model.mJoints[joint_id].mJointAxes[0][5]
|
||||
* q[model.mJoints[joint_id].q_index]
|
||||
)
|
||||
);
|
||||
|
|
@ -440,14 +455,14 @@ RBDL_DLLAPI void jcalc_X_lambda_S (
|
|||
model.X_lambda[joint_id] = XJ * model.X_T[joint_id];
|
||||
// Set the joint axis
|
||||
Vector3d trans = XJ.E * model.mJoints[joint_id].mJointAxes[0].block(3,0,3,1);
|
||||
|
||||
|
||||
model.S[joint_id] = SpatialVector(model.mJoints[joint_id].mJointAxes[0][0],
|
||||
model.mJoints[joint_id].mJointAxes[0][1],
|
||||
model.mJoints[joint_id].mJointAxes[0][2],
|
||||
trans[0], trans[1], trans[2]);
|
||||
} else if (model.mJoints[joint_id].mDoFCount == 1
|
||||
&& model.mJoints[joint_id].mJointType != JointTypeCustom){
|
||||
model.X_lambda[joint_id] =
|
||||
model.X_lambda[joint_id] =
|
||||
jcalc_XJ (model, joint_id, q) * model.X_T[joint_id];
|
||||
model.S[joint_id] = model.mJoints[joint_id].mJointAxes[0];
|
||||
} else if (model.mJoints[joint_id].mJointType == JointTypeSpherical) {
|
||||
|
|
@ -471,7 +486,7 @@ RBDL_DLLAPI void jcalc_X_lambda_S (
|
|||
double s2 = sin (q2);
|
||||
double c2 = cos (q2);
|
||||
|
||||
model.X_lambda[joint_id] = SpatialTransform (
|
||||
model.X_lambda[joint_id] = SpatialTransform (
|
||||
Matrix3d(
|
||||
c0 * c1, s0 * c1, -s1,
|
||||
c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
|
||||
|
|
@ -590,7 +605,7 @@ RBDL_DLLAPI void jcalc_X_lambda_S (
|
|||
model.multdof3_S[joint_id](5,2) = 1.;
|
||||
} else if (model.mJoints[joint_id].mJointType == JointTypeCustom) {
|
||||
const Joint &joint = model.mJoints[joint_id];
|
||||
CustomJoint *custom_joint
|
||||
CustomJoint *custom_joint
|
||||
= model.mCustomJoints[joint.custom_joint_index];
|
||||
|
||||
custom_joint->jcalc_X_lambda_S (model, joint_id, q);
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,39 +1,31 @@
|
|||
CMAKE_MINIMUM_REQUIRED (VERSION 3.0)
|
||||
|
||||
# Needed for UnitTest++
|
||||
LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../CMake )
|
||||
|
||||
# Look for unittest++
|
||||
FIND_PACKAGE (UnitTest++ REQUIRED)
|
||||
INCLUDE_DIRECTORIES (${UNITTEST++_INCLUDE_DIR})
|
||||
|
||||
SET ( TESTS_SRCS
|
||||
main.cc
|
||||
MathTests.cc
|
||||
SpatialAlgebraTests.cc
|
||||
MultiDofTests.cc
|
||||
KinematicsTests.cc
|
||||
BodyTests.cc
|
||||
ModelTests.cc
|
||||
FloatingBaseTests.cc
|
||||
CalcVelocitiesTests.cc
|
||||
CalcAccelerationsTests.cc
|
||||
DynamicsTests.cc
|
||||
InverseDynamicsTests.cc
|
||||
CompositeRigidBodyTests.cc
|
||||
ImpulsesTests.cc
|
||||
TwolegModelTests.cc
|
||||
ContactsTests.cc
|
||||
UtilsTests.cc
|
||||
SparseFactorizationTests.cc
|
||||
CustomJointSingleBodyTests.cc
|
||||
CustomJointMultiBodyTests.cc
|
||||
CustomConstraintsTests.cc
|
||||
InverseKinematicsTests.cc
|
||||
LoopConstraintsTests.cc
|
||||
ScrewJointTests.cc
|
||||
ForwardDynamicsConstraintsExternalForces.cc
|
||||
)
|
||||
MathTests.cc
|
||||
SpatialAlgebraTests.cc
|
||||
MultiDofTests.cc
|
||||
KinematicsTests.cc
|
||||
BodyTests.cc
|
||||
ModelTests.cc
|
||||
FloatingBaseTests.cc
|
||||
CalcVelocitiesTests.cc
|
||||
CalcAccelerationsTests.cc
|
||||
DynamicsTests.cc
|
||||
InverseDynamicsTests.cc
|
||||
CompositeRigidBodyTests.cc
|
||||
ImpulsesTests.cc
|
||||
TwolegModelTests.cc
|
||||
ContactsTests.cc
|
||||
UtilsTests.cc
|
||||
SparseFactorizationTests.cc
|
||||
CustomJointSingleBodyTests.cc
|
||||
CustomJointMultiBodyTests.cc
|
||||
CustomConstraintsTests.cc
|
||||
InverseKinematicsTests.cc
|
||||
LoopConstraintsTests.cc
|
||||
ScrewJointTests.cc
|
||||
ForwardDynamicsConstraintsExternalForces.cc
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES ( ../src/ )
|
||||
|
||||
|
|
@ -45,7 +37,7 @@ SET_TARGET_PROPERTIES ( ${PROJECT_EXECUTABLES} PROPERTIES
|
|||
CXX_EXTENSIONS OFF
|
||||
)
|
||||
|
||||
ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} )
|
||||
ADD_EXECUTABLE ( rbdl_tests ${TESTS_SRCS} main.cc)
|
||||
|
||||
SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES
|
||||
LINKER_LANGUAGE CXX
|
||||
|
|
@ -55,15 +47,16 @@ SET_TARGET_PROPERTIES ( rbdl_tests PROPERTIES
|
|||
CXX_EXTENSIONS OFF
|
||||
)
|
||||
|
||||
|
||||
SET (RBDL_LIBRARY rbdl)
|
||||
IF (RBDL_BUILD_STATIC)
|
||||
SET (RBDL_LIBRARY rbdl-static)
|
||||
ENDIF (RBDL_BUILD_STATIC)
|
||||
|
||||
|
||||
TARGET_LINK_LIBRARIES ( rbdl_tests
|
||||
${UNITTEST++_LIBRARY}
|
||||
${RBDL_LIBRARY}
|
||||
)
|
||||
${RBDL_LIBRARY}
|
||||
)
|
||||
|
||||
OPTION (RUN_AUTOMATIC_TESTS "Perform automatic tests after compilation?" OFF)
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
#include <UnitTest++.h>
|
||||
#include "rbdl_tests.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -100,10 +100,10 @@ struct FixedBase6DoF9DoF {
|
|||
ConstraintSet constraint_set;
|
||||
};
|
||||
|
||||
//
|
||||
// ForwardDynamicsConstraintsDirect
|
||||
//
|
||||
TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
|
||||
//
|
||||
// ForwardDynamicsConstraintsDirect
|
||||
//
|
||||
TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectSimple", "") {
|
||||
Model model;
|
||||
model.gravity = Vector3d (0., -9.81, 0.);
|
||||
Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
|
||||
|
|
@ -152,19 +152,14 @@ TEST ( TestForwardDynamicsConstraintsDirectSimple ) {
|
|||
|
||||
Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
Vector3d (0., 0., 0.).data(),
|
||||
point_acceleration.data(),
|
||||
3,
|
||||
TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
|
||||
|
||||
// cout << "LagrangianSimple Logoutput Start" << endl;
|
||||
// cout << LogOutput.str() << endl;
|
||||
// cout << "LagrangianSimple Logoutput End" << endl;
|
||||
}
|
||||
|
||||
TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
|
||||
TEST_CASE (__FILE__"_TestForwardDynamicsConstraintsDirectMoving", "") {
|
||||
Model model;
|
||||
model.gravity = Vector3d (0., -9.81, 0.);
|
||||
Body base_body (1., Vector3d (0., 0., 0.), Vector3d (1., 1., 1.));
|
||||
|
|
@ -215,12 +210,7 @@ TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
|
|||
|
||||
Vector3d point_acceleration = CalcPointAcceleration (model, Q, QDot, QDDot, contact_body_id, contact_point);
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
Vector3d (0., 0., 0.).data(),
|
||||
point_acceleration.data(),
|
||||
3,
|
||||
TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_acceleration, TEST_PREC, TEST_PREC));
|
||||
|
||||
// cout << "LagrangianSimple Logoutput Start" << endl;
|
||||
// cout << LogOutput.str() << endl;
|
||||
|
|
@ -229,8 +219,8 @@ TEST ( TestForwardDynamicsConstraintsDirectMoving ) {
|
|||
|
||||
//
|
||||
// ForwardDynamicsContacts
|
||||
//
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
|
||||
//
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContact", "") {
|
||||
contact_normal.set (0., 1., 0.);
|
||||
constraint_set.AddContactConstraint (contact_body_id, contact_point, contact_normal);
|
||||
ConstraintSet constraint_set_lagrangian = constraint_set.Copy();
|
||||
|
|
@ -255,13 +245,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContact) {
|
|||
point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
|
||||
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
|
||||
|
||||
CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
|
||||
CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts), TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContactRotated", "") {
|
||||
Q[0] = 0.6;
|
||||
Q[3] = M_PI * 0.6;
|
||||
Q[4] = 0.1;
|
||||
|
|
@ -289,10 +279,10 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
|
|||
point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
|
||||
point_accel_contacts_opt = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts_opt, contact_body_id, contact_point, true);
|
||||
|
||||
CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
|
||||
CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts_opt), TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts_opt.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts_opt.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts_opt), TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts_opt, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts_opt, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
//
|
||||
|
|
@ -301,8 +291,8 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotated) {
|
|||
// - ForwardDynamcsContactsOpt
|
||||
// for the example model in FixedBase6DoF and a moving state (i.e. a
|
||||
// nonzero QDot)
|
||||
//
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving) {
|
||||
//
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsSingleContactRotatedMoving", "") {
|
||||
Q[0] = 0.6;
|
||||
Q[3] = M_PI * 0.6;
|
||||
Q[4] = 0.1;
|
||||
|
|
@ -335,14 +325,13 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsSingleContactRotatedMoving)
|
|||
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
|
||||
|
||||
// check whether FDContactsLagrangian and FDContactsOld match
|
||||
CHECK_CLOSE (constraint_set_lagrangian.force[0], constraint_set.force[0], TEST_PREC);
|
||||
|
||||
CHECK_CLOSE (contact_normal.dot(point_accel_lagrangian), contact_normal.dot(point_accel_contacts), TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force[0], IsClose(constraint_set.force[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (contact_normal.dot(point_accel_lagrangian), IsClose(contact_normal.dot(point_accel_contacts), TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptDoubleContact", "") {
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
||||
constraint_set.AddContactConstraint (contact_body_id, Vector3d (1., 0., 0.), contact_normal);
|
||||
|
|
@ -368,20 +357,16 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContact) {
|
|||
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
|
||||
|
||||
// check whether FDContactsLagrangian and FDContacts match
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
// check whether the point accelerations match
|
||||
CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
|
||||
|
||||
// check whether the generalized accelerations match
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptDoubleContactRepeated", "") {
|
||||
// makes sure that all variables in the constraint set gets reset
|
||||
// properly when making repeated calls to ForwardDynamicsContacts.
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
|
@ -412,21 +397,18 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptDoubleContactRepeated) {
|
|||
point_accel_lagrangian = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point, true);
|
||||
point_accel_contacts = CalcPointAcceleration (*model, Q, QDot, QDDot_contacts, contact_body_id, contact_point, true);
|
||||
|
||||
|
||||
// check whether FDContactsLagrangian and FDContacts match
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
// check whether the point accelerations match
|
||||
CHECK_ARRAY_CLOSE (point_accel_lagrangian.data(), point_accel_contacts.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (point_accel_lagrangian, AllCloseVector(point_accel_contacts, TEST_PREC, TEST_PREC));
|
||||
|
||||
// check whether the generalized accelerations match
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContact", "") {
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
||||
constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
|
||||
|
|
@ -464,19 +446,15 @@ TEST_FIXTURE (FixedBase6DoF, ForwardDynamicsContactsOptMultipleContact) {
|
|||
|
||||
// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << endl;
|
||||
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot_contacts.data(), QDDot_lagrangian.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot_contacts, TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF9DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMoving", "") {
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
||||
constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
|
||||
|
|
@ -517,27 +495,23 @@ TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMulti
|
|||
ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
|
||||
// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
|
||||
point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF9DoF, __FILE__"_ForwardDynamicsContactsOptMultipleContactsMultipleBodiesMovingAlternate", "") {
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
||||
constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
|
||||
|
|
@ -584,27 +558,23 @@ TEST_FIXTURE (FixedBase6DoF9DoF, ForwardDynamicsContactsOptMultipleContactsMulti
|
|||
ForwardDynamicsConstraintsDirect (*model, Q, QDot, Tau, constraint_set_lagrangian, QDDot_lagrangian);
|
||||
// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
|
||||
point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleContactsFloatingBase) {
|
||||
TEST_CASE_METHOD (FixedBase6DoF12DoFFloatingBase, __FILE__"_ForwardDynamicsContactsMultipleContactsFloatingBase", "") {
|
||||
ConstraintSet constraint_set_lagrangian;
|
||||
|
||||
constraint_set.AddContactConstraint (contact_body_id, contact_point, Vector3d (1., 0., 0.));
|
||||
|
|
@ -653,27 +623,23 @@ TEST_FIXTURE (FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsMultipleCon
|
|||
// cout << "Lagrangian contact force " << contact_data_lagrangian[0].force << ", " << contact_data_lagrangian[1].force << ", " << contact_data_lagrangian[2].force << endl;
|
||||
// cout << LogOutput.str() << endl;
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
constraint_set_lagrangian.force.data(),
|
||||
constraint_set.force.data(),
|
||||
constraint_set.size(), TEST_PREC
|
||||
);
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (constraint_set_lagrangian.force, AllCloseVector(constraint_set.force, TEST_PREC, TEST_PREC));
|
||||
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
point_accel_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, contact_body_id, contact_point);
|
||||
point_accel_2_c = CalcPointAcceleration (*model, Q, QDot, QDDot_lagrangian, child_2_id, contact_point);
|
||||
|
||||
CHECK_CLOSE (0., point_accel_c[0], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_c[1], TEST_PREC);
|
||||
CHECK_CLOSE (0., point_accel_2_c[1], TEST_PREC);
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[0], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_c[1], TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (0., IsClose(point_accel_2_c[1], TEST_PREC, TEST_PREC));
|
||||
|
||||
CHECK_ARRAY_CLOSE (QDDot_lagrangian.data(), QDDot.data(), QDDot.size(), TEST_PREC);
|
||||
REQUIRE_THAT (QDDot_lagrangian, AllCloseVector(QDDot, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) {
|
||||
TEST_CASE_METHOD (Human36, __FILE__"_ForwardDynamicsContactsFixedBody", "") {
|
||||
VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size()));
|
||||
VectorNd qddot_sparse (VectorNd::Zero(qddot.size()));
|
||||
|
||||
|
|
@ -687,11 +653,11 @@ TEST_FIXTURE (Human36, ForwardDynamicsContactsFixedBody) {
|
|||
ForwardDynamicsConstraintsRangeSpaceSparse (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot_sparse);
|
||||
ForwardDynamicsContactsKokkevis (*model_3dof, q, qdot, tau, constraint_upper_trunk, qddot);
|
||||
|
||||
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.);
|
||||
CHECK_ARRAY_CLOSE (qddot_lagrangian.data(), qddot_sparse.data(), qddot_lagrangian.size(), TEST_PREC * qddot_lagrangian.norm() * 10.);
|
||||
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot, TEST_PREC * qddot_lagrangian.norm() * 10., TEST_PREC * qddot_lagrangian.norm() * 10.));
|
||||
REQUIRE_THAT (qddot_lagrangian, AllCloseVector(qddot_sparse, TEST_PREC * qddot_lagrangian.norm() * 10., TEST_PREC * qddot_lagrangian.norm() * 10.));
|
||||
}
|
||||
|
||||
TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) {
|
||||
TEST_CASE_METHOD (Human36, __FILE__"_ForwardDynamicsContactsImpulses", "") {
|
||||
VectorNd qddot_lagrangian (VectorNd::Zero(qddot.size()));
|
||||
|
||||
for (unsigned int i = 0; i < q.size(); i++) {
|
||||
|
|
@ -719,6 +685,6 @@ TEST_FIXTURE (Human36, ForwardDynamicsContactsImpulses) {
|
|||
Vector3d heel_left_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootLeft], heel_point);
|
||||
Vector3d heel_right_velocity = CalcPointVelocity (*model_3dof, q, qdotplus, body_id_3dof[BodyFootRight], heel_point);
|
||||
|
||||
CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_left_velocity.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (Vector3d(0., 0., 0.).data(), heel_right_velocity.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_left_velocity, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (Vector3d(0., 0., 0.), AllCloseVector(heel_right_velocity, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
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){
|
||||
REQUIRE_THAT (dba.cs.constraintAxis[i][j], IsClose(dbcc.cs.constraintAxis[i][j], TEST_PREC, 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//
|
||||
|
|
|
|||
|
|
@ -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,15 +480,8 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
|
|||
custom_body_id.at(idx),
|
||||
Gcus);
|
||||
|
||||
for(int i=0; i<6;++i){
|
||||
for(unsigned int j = 0; j < dof; ++j){
|
||||
CHECK_CLOSE (
|
||||
Gref(i,j),
|
||||
Gcus(i,j),
|
||||
TEST_PREC);
|
||||
}
|
||||
}
|
||||
|
||||
REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
|
||||
|
||||
//Check the 6d point Jacobian
|
||||
Vector3d point_position (1.1, 1.2, 2.1);
|
||||
|
||||
|
|
@ -535,16 +497,7 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
|
|||
point_position,
|
||||
Gcus);
|
||||
|
||||
for(int i=0; i<6;++i){
|
||||
for(unsigned int j = 0; j < dof; ++j){
|
||||
CHECK_CLOSE (
|
||||
Gref(i,j),
|
||||
Gcus(i,j),
|
||||
TEST_PREC);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
REQUIRE_THAT (Gref, AllCloseMatrix(Gcus, TEST_PREC, TEST_PREC));
|
||||
|
||||
//Check the 3d point Jacobian
|
||||
MatrixNd GrefPt = MatrixNd::Constant (3,
|
||||
|
|
@ -554,8 +507,6 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
|
|||
reference_model.at(idx).dof_count,
|
||||
0.);
|
||||
|
||||
|
||||
|
||||
CalcPointJacobian (reference_model.at(idx),
|
||||
q.at(idx),
|
||||
reference_body_id.at(idx),
|
||||
|
|
@ -568,20 +519,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
|
|||
point_position,
|
||||
GcusPt);
|
||||
|
||||
for(int i=0; i<3;++i){
|
||||
for(unsigned int j = 0; j < dof; ++j){
|
||||
CHECK_CLOSE (
|
||||
GrefPt(i,j),
|
||||
GcusPt(i,j),
|
||||
TEST_PREC);
|
||||
}
|
||||
}
|
||||
REQUIRE_THAT (GrefPt, AllCloseMatrix(GcusPt, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) {
|
||||
|
||||
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_InverseDynamics", "") {
|
||||
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
|
||||
unsigned int dof = reference_model.at(idx).dof_count;
|
||||
for (unsigned int i = 0; i < dof; i++) {
|
||||
|
|
@ -609,17 +551,12 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) {
|
|||
|
||||
VectorNd tauErr = tauRef-tauCus;
|
||||
|
||||
CHECK_ARRAY_CLOSE (
|
||||
tauRef.data(),
|
||||
tauCus.data(),
|
||||
tauRef.rows(),
|
||||
TEST_PREC);
|
||||
REQUIRE_THAT (tauRef, AllCloseVector(tauCus, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) {
|
||||
|
||||
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_CompositeRigidBodyAlgorithm", "") {
|
||||
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
|
||||
unsigned int dof = reference_model.at(idx).dof_count;
|
||||
for (unsigned int i = 0; i < dof; i++) {
|
||||
|
|
@ -683,15 +620,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) {
|
|||
c_cus * -1. + tau.at(idx),
|
||||
qddot_crba_cus);
|
||||
|
||||
CHECK_ARRAY_CLOSE(qddot_crba_ref.data(),
|
||||
qddot_crba_cus.data(),
|
||||
dof,
|
||||
TEST_PREC);
|
||||
REQUIRE_THAT(qddot_crba_ref, AllCloseVector(qddot_crba_cus, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
}
|
||||
|
||||
TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) {
|
||||
|
||||
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_ForwardDynamics", "") {
|
||||
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
|
||||
unsigned int dof = reference_model.at(idx).dof_count;
|
||||
for (unsigned int i = 0; i < dof; i++) {
|
||||
|
|
@ -717,15 +650,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) {
|
|||
tau.at(idx),
|
||||
qddotCus);
|
||||
|
||||
CHECK_ARRAY_CLOSE ( qddotRef.data(),
|
||||
qddotCus.data(),
|
||||
dof,
|
||||
TEST_PREC);
|
||||
REQUIRE_THAT ( qddotRef, AllCloseVector(qddotCus, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) {
|
||||
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_CalcMInvTimestau", "") {
|
||||
|
||||
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
|
||||
unsigned int dof = reference_model.at(idx).dof_count;
|
||||
|
|
@ -753,16 +682,11 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) {
|
|||
qddot_minv_cus,
|
||||
true);
|
||||
//check.
|
||||
CHECK_ARRAY_CLOSE(qddot_minv_ref.data(),
|
||||
qddot_minv_cus.data(),
|
||||
dof,
|
||||
TEST_PREC);
|
||||
REQUIRE_THAT(qddot_minv_ref, AllCloseVector(qddot_minv_cus, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){
|
||||
|
||||
TEST_CASE_METHOD (CustomJointSingleBodyFixture, __FILE__"_ForwardDynamicsContactsKokkevis", ""){
|
||||
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
|
||||
unsigned int dof = reference_model.at(idx).dof_count;
|
||||
//Adding a 1 constraint to a system with 1 dof is
|
||||
|
|
@ -842,18 +766,8 @@ TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){
|
|||
VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
|
||||
VectorNd qddot_error = qddot_ref - qddot_cus;
|
||||
|
||||
CHECK_ARRAY_CLOSE (qdot_plus_ref.data(),
|
||||
qdot_plus_cus.data(),
|
||||
dof,
|
||||
TEST_PREC);
|
||||
|
||||
CHECK_ARRAY_CLOSE (qddot_ref.data(),
|
||||
qddot_cus.data(),
|
||||
dof,
|
||||
TEST_PREC);
|
||||
REQUIRE_THAT (qdot_plus_ref, AllCloseVector(qdot_plus_cus, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (qddot_ref, AllCloseVector(qddot_cus, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ struct CustomEulerZYXJoint : public CustomJoint {
|
|||
const Math::VectorNd &q
|
||||
) {
|
||||
// TODO
|
||||
assert (false && "Not yet implemented!");
|
||||
REQUIRE (false && "Not yet implemented!");
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
#include <UnitTest++.h>
|
||||
#include "rbdl_tests.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -75,10 +75,10 @@ struct ImpulsesFixture {
|
|||
ConstraintSet constraint_set;
|
||||
};
|
||||
|
||||
TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.);
|
||||
TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulse", "") {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.);
|
||||
|
||||
constraint_set.Bind (*model);
|
||||
|
||||
|
|
@ -109,13 +109,13 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulse) {
|
|||
}
|
||||
|
||||
// cout << "Point Velocity = " << point_velocity << endl;
|
||||
CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.);
|
||||
TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRotated", "") {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 0.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 0.);
|
||||
|
||||
constraint_set.Bind (*model);
|
||||
|
||||
|
|
@ -152,13 +152,13 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotated) {
|
|||
}
|
||||
|
||||
// cout << "Point Velocity = " << point_velocity << endl;
|
||||
CHECK_ARRAY_CLOSE (Vector3d (0., 0., 0.).data(), point_velocity.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (Vector3d (0., 0., 0.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.);
|
||||
TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRotatedCollisionVelocity", "") {
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (1., 0., 0.), NULL, 1.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 1., 0.), NULL, 2.);
|
||||
constraint_set.AddContactConstraint(contact_body_id, contact_point, Vector3d (0., 0., 1.), NULL, 3.);
|
||||
|
||||
constraint_set.Bind (*model);
|
||||
|
||||
|
|
@ -197,10 +197,10 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRotatedCollisionVelocity) {
|
|||
}
|
||||
|
||||
// cout << "Point Velocity = " << point_velocity << endl;
|
||||
CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) {
|
||||
TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseRangeSpaceSparse", "") {
|
||||
Q[0] = 0.2;
|
||||
Q[1] = -0.5;
|
||||
Q[2] = 0.1;
|
||||
|
|
@ -235,12 +235,11 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseRangeSpaceSparse) {
|
|||
Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
|
||||
Vector3d point_velocity_rangespace = CalcPointVelocity (*model, Q, qdot_post_rangespace, contact_body_id, contact_point, true);
|
||||
|
||||
CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_rangespace.data(), qdot_post_direct.rows(), TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_rangespace.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_rangespace, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_rangespace, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
||||
TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) {
|
||||
TEST_CASE_METHOD (ImpulsesFixture, __FILE__"_TestContactImpulseNullSpace", "") {
|
||||
Q[0] = 0.2;
|
||||
Q[1] = -0.5;
|
||||
Q[2] = 0.1;
|
||||
|
|
@ -275,7 +274,6 @@ TEST_FIXTURE(ImpulsesFixture, TestContactImpulseNullSpace) {
|
|||
Vector3d point_velocity_direct = CalcPointVelocity (*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
|
||||
Vector3d point_velocity_nullspace = CalcPointVelocity (*model, Q, qdot_post_nullspace, contact_body_id, contact_point, true);
|
||||
|
||||
CHECK_ARRAY_CLOSE (qdot_post_direct.data(), qdot_post_nullspace.data(), qdot_post_direct.rows(), TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_direct.data(), 3, TEST_PREC);
|
||||
CHECK_ARRAY_CLOSE (Vector3d (1., 2., 3.).data(), point_velocity_nullspace.data(), 3, TEST_PREC);
|
||||
REQUIRE_THAT (qdot_post_direct, AllCloseVector(qdot_post_nullspace, TEST_PREC, TEST_PREC));
|
||||
REQUIRE_THAT (Vector3d (1., 2., 3.), AllCloseVector(point_velocity_nullspace, TEST_PREC, TEST_PREC));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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()));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
@ -82,12 +82,11 @@ TEST_FIXTURE ( ScrewJoint1DofFixedBase, UpdateKinematics ) {
|
|||
|
||||
v0 = model->v[roller] - v0;
|
||||
v0 /= epsilon;
|
||||
|
||||
CHECK_ARRAY_CLOSE (a0.data(),v0.data(), 6, 1e-5); //finite diff vs. analytical derivative
|
||||
|
||||
|
||||
REQUIRE_THAT (a0, AllCloseVector(v0, 1e-5, 1e-5)); //finite diff vs. analytical derivative
|
||||
}
|
||||
|
||||
TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) {
|
||||
TEST_CASE_METHOD (ScrewJoint1DofFixedBase, __FILE__"_Jacobians", "") {
|
||||
q[0] = 1;
|
||||
qdot[0] = 0;
|
||||
qddot[0] = 9;
|
||||
|
|
@ -99,13 +98,14 @@ TEST_FIXTURE ( ScrewJoint1DofFixedBase, Jacobians ) {
|
|||
|
||||
refPtBaseCoord = CalcBodyToBaseCoordinates(*model, q, roller, refPt);
|
||||
|
||||
CHECK_ARRAY_EQUAL (Vector3d(1+cos(1), sin(1), 3).data(), refPtBaseCoord.data(), 3);
|
||||
|
||||
REQUIRE_THAT (Vector3d(1+cos(1), sin(1), 3), AllCloseVector(refPtBaseCoord));
|
||||
|
||||
CalcPointJacobian(*model, q, roller, refPt, GrefPt);
|
||||
|
||||
Gexpected(0,0) = 1 - sin(1);
|
||||
Gexpected(1,0) = cos(1);
|
||||
Gexpected(2,0) = 0;
|
||||
|
||||
CHECK_ARRAY_EQUAL (Gexpected.data(), GrefPt.data(), 3);
|
||||
REQUIRE_THAT (Gexpected, AllCloseMatrix(GrefPt));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue