rbdlsim/3rdparty/rbdl/tests/FloatingBaseTests.cc

551 lines
18 KiB
C++

#include <UnitTest++.h>
#include <iostream>
#include "rbdl/rbdl_mathutils.h"
#include "rbdl/Logging.h"
#include "rbdl/Model.h"
#include "rbdl/Kinematics.h"
#include "rbdl/Dynamics.h"
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-14;
struct FloatingBaseFixture {
FloatingBaseFixture () {
ClearLogOutput();
model = new Model;
model->gravity = Vector3d (0., -9.81, 0.);
base = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
}
~FloatingBaseFixture () {
delete model;
}
Model *model;
Body base;
unsigned int base_body_id;
VectorNd q, qdot, qddot, tau;
};
TEST_FIXTURE ( FloatingBaseFixture, TestCalcPointTransformation ) {
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
q = VectorNd::Constant(model->dof_count, 0.);
qdot = VectorNd::Constant(model->dof_count, 0.);
qddot = VectorNd::Constant(model->dof_count, 0.);
tau = VectorNd::Constant(model->dof_count, 0.);
q[1] = 1.;
ForwardDynamics (*model, q, qdot, tau, qddot);
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);
}
TEST_FIXTURE(FloatingBaseFixture, TestCalcDynamicFloatingBaseDoubleImplicit) {
// floating base
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
// body_a
Body body_a (1., Vector3d (1., 0., 0), Vector3d (1., 1., 1.));
Joint joint_a ( SpatialVector (0., 0., 1., 0., 0., 0.));
model->AddBody(base_body_id, Xtrans(Vector3d(2., 0., 0.)), joint_a, body_a);
// Initialization of the input vectors
VectorNd Q = VectorNd::Zero ((size_t) model->dof_count);
VectorNd QDot = VectorNd::Zero ((size_t) model->dof_count);
VectorNd QDDot = VectorNd::Zero ((size_t) model->dof_count);
VectorNd Tau = VectorNd::Zero ((size_t) model->dof_count);
ForwardDynamics(*model, Q, QDot, Tau, QDDot);
unsigned int i;
for (i = 0; i < QDDot.size(); i++) {
LOG << "QDDot[" << i << "] = " << QDDot[i] << endl;
}
for (i = 0; i < model->a.size(); i++) {
LOG << "a[" << i << "] = " << model->a.at(i) << endl;
}
// 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);
// We rotate the base... let's see what happens...
Q[3] = 0.8;
ForwardDynamics(*model, Q, QDot, Tau, QDDot);
for (i = 0; i < QDDot.size(); i++) {
LOG << "QDDot[" << i << "] = " << QDDot[i] << endl;
}
for (i = 0; i < model->a.size(); i++) {
LOG << "a[" << i << "] = " << model->a.at(i) << endl;
}
// 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);
// We apply a torqe let's see what happens...
Q[3] = 0.;
/*
rot_B[0] = 0.0;
X_B = XtransRotZYXEuler(pos_B, rot_B);
*/
Tau[6] = 1.;
ForwardDynamics(*model, Q, QDot, Tau, QDDot);
for (i = 0; i < QDDot.size(); i++) {
LOG << "QDDot[" << i << "] = " << QDDot[i] << endl;
}
for (i = 0; i < model->a.size(); i++) {
LOG << "a[" << i << "] = " << model->a.at(i) << endl;
}
// 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);
}
TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityFloatingBaseSimple) {
// floating base
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
VectorNd Q = VectorNd::Zero (model->dof_count);
VectorNd QDot = VectorNd::Zero (model->dof_count);
VectorNd QDDot = VectorNd::Zero (model->dof_count);
VectorNd Tau = VectorNd::Zero (model->dof_count);
unsigned int ref_body_id = base_body_id;
// first we calculate the velocity when moving along the X axis
QDot[0] = 1.;
Vector3d point_position(1., 0., 0.);
Vector3d point_velocity;
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);
LOG << "Point velocity = " << point_velocity << endl;
// cout << LogOutput.str() << endl;
ClearLogOutput();
// Now we calculate the velocity when rotating around the Z axis
QDot[0] = 0.;
QDot[3] = 1.;
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);
LOG << "Point velocity = " << point_velocity << endl;
// cout << LogOutput.str() << endl;
// Now we calculate the velocity when rotating around the Z axis and the
// base is rotated around the z axis by 90 degrees
ClearLogOutput();
Q[3] = M_PI * 0.5;
QDot[3] = 1.;
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);
LOG << "Point velocity = " << point_velocity << endl;
// cout << LogOutput.str() << endl;
}
TEST_FIXTURE(FloatingBaseFixture, TestCalcPointVelocityCustom) {
// floating base
base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
VectorNd q = VectorNd::Zero (model->dof_count);
VectorNd qdot = VectorNd::Zero (model->dof_count);
VectorNd qddot = VectorNd::Zero (model->dof_count);
VectorNd tau = VectorNd::Zero (model->dof_count);
unsigned int ref_body_id = base_body_id;
q[0] = 0.1;
q[1] = 1.1;
q[2] = 1.2;
q[3] = 1.3;
q[4] = 1.5;
q[5] = 1.7;
qdot[0] = 0.1;
qdot[1] = 1.1;
qdot[2] = 1.2;
qdot[3] = 1.3;
qdot[4] = 1.5;
qdot[5] = 1.7;
// first we calculate the velocity when rotating around the Z axis
Vector3d point_body_position (1., 0., 0.);
Vector3d point_base_position;
Vector3d point_base_velocity;
Vector3d point_base_velocity_reference;
ForwardDynamics(*model, q, qdot, tau, qddot);
point_base_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position);
point_base_velocity_reference = Vector3d (
-3.888503432977729e-01,
-3.171179347202455e-01,
1.093894197498446e+00
);
CHECK_ARRAY_CLOSE (point_base_velocity_reference.data(), point_base_velocity.data(), 3, TEST_PREC);
}
/** \brief Compares computation of acceleration values for zero qddot
*
* Ensures that computation of position, velocity, and acceleration of a
* point produce the same values as in an equivalent model that was
* created with the HuMAnS toolbox
* http://www.inrialpes.fr/bipop/software/humans/ .
* Here we omit the term of the generalized acceleration by setting qddot
* to zero.
*/
TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationNoQDDot) {
// floating base
base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
VectorNd q = VectorNd::Zero (model->dof_count);
VectorNd qdot = VectorNd::Zero (model->dof_count);
VectorNd qddot = VectorNd::Zero (model->dof_count);
VectorNd tau = VectorNd::Zero (model->dof_count);
unsigned int ref_body_id = base_body_id;
q[0] = 0.1;
q[1] = 1.1;
q[2] = 1.2;
q[3] = 1.3;
q[4] = 1.5;
q[5] = 1.7;
qdot[0] = 0.1;
qdot[1] = 1.1;
qdot[2] = 1.2;
qdot[3] = 1.3;
qdot[4] = 1.5;
qdot[5] = 1.7;
// first we calculate the velocity when rotating around the Z axis
Vector3d point_body_position (-1.9, -1.8, 0.);
Vector3d point_world_position;
Vector3d point_world_velocity;
Vector3d point_world_acceleration;
// call ForwardDynamics to update the model
ForwardDynamics(*model, q, qdot, tau, qddot);
qddot = VectorNd::Zero (qddot.size());
qdot = qdot;
point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false);
point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position);
// we set the generalized acceleration to zero
ClearLogOutput();
point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position);
Vector3d humans_point_position (
-6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00
);
Vector3d humans_point_velocity (
3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00
);
Vector3d humans_point_acceleration (
-5.302760158847160e+00, 6.541369639625232e+00, -4.795115077652286e+00
);
// cout << LogOutput.str() << endl;
//
// cout << "q = " << q << endl;
// cout << "qdot = " << qdot << endl;
// cout << "qddot = " << qddot << endl;
//
// cout << "body_coords = " << point_body_position << endl;
// cout << "world_pos = " << point_world_position << endl;
// 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);
}
/** \brief Compares computation of acceleration values for zero q and qdot
*
* Ensures that computation of position, velocity, and acceleration of a
* point produce the same values as in an equivalent model that was
* created with the HuMAnS toolbox
* http://www.inrialpes.fr/bipop/software/humans/ .
*
* Here we set q and qdot to zero and only take into account values that
* are dependent on qddot.
*/
TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationOnlyQDDot) {
// floating base
base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
VectorNd q = VectorNd::Zero (model->dof_count);
VectorNd qdot = VectorNd::Zero (model->dof_count);
VectorNd qddot = VectorNd::Zero (model->dof_count);
VectorNd tau = VectorNd::Zero (model->dof_count);
unsigned int ref_body_id = base_body_id;
// first we calculate the velocity when rotating around the Z axis
Vector3d point_body_position (-1.9, -1.8, 0.);
Vector3d point_world_position;
Vector3d point_world_velocity;
Vector3d point_world_acceleration;
ForwardDynamics(*model, q, qdot, tau, qddot);
qddot = VectorNd::Zero (qddot.size());
qddot[0] = 0.1;
qddot[1] = 1.1;
qddot[2] = 1.2;
qddot[3] = 1.3;
qddot[4] = 1.5;
qddot[5] = 1.7;
// cout << "ref_body_id = " << ref_body_id << endl;
// cout << "point_body_position = " << point_body_position << endl;
point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false);
point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position);
ClearLogOutput();
point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position);
Vector3d humans_point_position (
-1.900000000000000e+00, -1.800000000000000e+00, 0.000000000000000e+00
);
Vector3d humans_point_velocity (
0.000000000000000e+00, 0.000000000000000e+00, 0.000000000000000e+00
);
Vector3d humans_point_acceleration (
2.440000000000000e+00, -1.370000000000000e+00, 9.899999999999999e-01
);
// cout << LogOutput.str() << endl;
//
// cout << "q = " << q << endl;
// cout << "qdot = " << qdot << endl;
// cout << "qddot = " << qddot << endl;
//
// cout << "body_coords = " << point_body_position << endl;
// cout << "world_pos = " << point_world_position << endl;
// 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);
}
/** \brief Compares computation of acceleration values for zero q and qdot
*
* Ensures that computation of position, velocity, and acceleration of a
* point produce the same values as in an equivalent model that was
* created with the HuMAnS toolbox
* http://www.inrialpes.fr/bipop/software/humans/ .
*
* Here we set q and qdot to zero and only take into account values that
* are dependent on qddot.
*/
TEST_FIXTURE(FloatingBaseFixture, TestCalcPointAccelerationFull) {
// floating base
base = Body (1., Vector3d (0., 1., 0.), Vector3d (1., 1., 1.));
base_body_id = model->AddBody (0, SpatialTransform(),
Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 0., 1., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.)
),
base);
VectorNd q = VectorNd::Zero (model->dof_count);
VectorNd qdot = VectorNd::Zero (model->dof_count);
VectorNd qddot = VectorNd::Zero (model->dof_count);
VectorNd tau = VectorNd::Zero (model->dof_count);
unsigned int ref_body_id = base_body_id;
// first we calculate the velocity when rotating around the Z axis
Vector3d point_body_position (-1.9, -1.8, 0.);
Vector3d point_world_position;
Vector3d point_world_velocity;
Vector3d point_world_acceleration;
q[0] = 0.1;
q[1] = 1.1;
q[2] = 1.2;
q[3] = 1.3;
q[4] = 1.5;
q[5] = 1.7;
qdot[0] = 0.1;
qdot[1] = 1.1;
qdot[2] = 1.2;
qdot[3] = 1.3;
qdot[4] = 1.5;
qdot[5] = 1.7;
ForwardDynamics(*model, q, qdot, tau, qddot);
qddot[0] = 0.1;
qddot[1] = 1.1;
qddot[2] = 1.2;
qddot[3] = 1.3;
qddot[4] = 1.5;
qddot[5] = 1.7;
// cout << "ref_body_id = " << ref_body_id << endl;
// cout << "point_body_position = " << point_body_position << endl;
point_world_position = CalcBodyToBaseCoordinates (*model, q, ref_body_id, point_body_position, false);
point_world_velocity = CalcPointVelocity (*model, q, qdot, ref_body_id, point_body_position);
ClearLogOutput();
point_world_acceleration = CalcPointAcceleration (*model, q, qdot, qddot, ref_body_id, point_body_position);
Vector3d humans_point_position (
-6.357089363622626e-01, -6.831041744630977e-01, 2.968974805916970e+00
);
Vector3d humans_point_velocity (
3.091226260907569e-01, 3.891012095550828e+00, 4.100277995030419e+00
);
Vector3d humans_point_acceleration (
-4.993637532756404e+00, 1.043238173517606e+01, -6.948370826218673e-01
);
// cout << LogOutput.str() << endl;
//
// cout << "q = " << q << endl;
// cout << "qdot = " << qdot << endl;
// cout << "qddot = " << qddot << endl;
//
// cout << "body_coords = " << point_body_position << endl;
// cout << "world_pos = " << point_world_position << endl;
// 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);
}