633 lines
22 KiB
C++
633 lines
22 KiB
C++
#include <UnitTest++.h>
|
|
|
|
#include <iostream>
|
|
|
|
#include "Fixtures.h"
|
|
#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 ModelFixture {
|
|
ModelFixture () {
|
|
ClearLogOutput();
|
|
model = new Model;
|
|
model->gravity = Vector3d (0., -9.81, 0.);
|
|
}
|
|
~ModelFixture () {
|
|
delete model;
|
|
}
|
|
Model *model;
|
|
};
|
|
|
|
TEST_FIXTURE(ModelFixture, TestInit) {
|
|
CHECK_EQUAL (1u, model->lambda.size());
|
|
CHECK_EQUAL (1u, model->mu.size());
|
|
CHECK_EQUAL (0u, model->dof_count);
|
|
|
|
CHECK_EQUAL (0u, model->q_size);
|
|
CHECK_EQUAL (0u, model->qdot_size);
|
|
|
|
CHECK_EQUAL (1u, model->v.size());
|
|
CHECK_EQUAL (1u, model->a.size());
|
|
|
|
CHECK_EQUAL (1u, model->mJoints.size());
|
|
CHECK_EQUAL (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());
|
|
|
|
CHECK_EQUAL (1u, model->X_lambda.size());
|
|
CHECK_EQUAL (1u, model->X_base.size());
|
|
CHECK_EQUAL (1u, model->mBodies.size());
|
|
}
|
|
|
|
TEST_FIXTURE(ModelFixture, 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);
|
|
|
|
CHECK_EQUAL (2u, model->v.size());
|
|
CHECK_EQUAL (2u, model->a.size());
|
|
|
|
CHECK_EQUAL (2u, model->mJoints.size());
|
|
CHECK_EQUAL (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());
|
|
|
|
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());
|
|
}
|
|
|
|
TEST_FIXTURE(ModelFixture, 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);
|
|
|
|
CHECK_EQUAL (3u, model->v.size());
|
|
CHECK_EQUAL (3u, model->a.size());
|
|
|
|
CHECK_EQUAL (3u, model->mJoints.size());
|
|
CHECK_EQUAL (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());
|
|
|
|
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());
|
|
}
|
|
|
|
/** \brief Tests whether the joint and body information stored in the Model are computed correctly
|
|
*/
|
|
TEST_FIXTURE(ModelFixture, 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]);
|
|
|
|
// \Todo: Dynamic properties
|
|
}
|
|
|
|
TEST_FIXTURE(ModelFixture, TestAddBodyTestBodyName) {
|
|
Body body;
|
|
Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint, body, "mybody");
|
|
|
|
unsigned int body_id = model->GetBodyId("mybody");
|
|
|
|
CHECK_EQUAL (1u, body_id);
|
|
CHECK_EQUAL (std::numeric_limits<unsigned int>::max(), model->GetBodyId("unknownbody"));
|
|
}
|
|
|
|
TEST_FIXTURE(ModelFixture, TestjcalcSimple) {
|
|
Body body;
|
|
Joint joint ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, body);
|
|
|
|
VectorNd Q = VectorNd::Zero (model->q_size);
|
|
VectorNd QDot = VectorNd::Zero (model->q_size);
|
|
|
|
QDot[0] = 1.;
|
|
jcalc (*model, 1, Q, QDot);
|
|
|
|
SpatialMatrix test_matrix (
|
|
1., 0., 0., 0., 0., 0.,
|
|
0., 1., 0., 0., 0., 0.,
|
|
0., 0., 1., 0., 0., 0.,
|
|
0., 0., 0., 1., 0., 0.,
|
|
0., 0., 0., 0., 1., 0.,
|
|
0., 0., 0., 0., 0., 1.
|
|
);
|
|
SpatialVector test_vector (
|
|
0., 0., 1., 0., 0., 0.
|
|
);
|
|
SpatialVector test_joint_axis (
|
|
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]);
|
|
|
|
Q[0] = M_PI * 0.5;
|
|
QDot[0] = 1.;
|
|
|
|
jcalc (*model, 1, Q, QDot);
|
|
|
|
test_matrix <<
|
|
0., 1., 0., 0., 0., 0.,
|
|
-1., 0., 0., 0., 0., 0.,
|
|
0., 0., 1., 0., 0., 0.,
|
|
0., 0., 0., 0., 1., 0.,
|
|
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]);
|
|
}
|
|
|
|
TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
|
|
Body body;
|
|
|
|
unsigned int 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.)
|
|
),
|
|
body);
|
|
|
|
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);
|
|
|
|
Vector3d base_coords (0., 0., 0.);
|
|
Vector3d body_coords;
|
|
Vector3d base_coords_back;
|
|
|
|
UpdateKinematics (*model, q, qdot, qddot);
|
|
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);
|
|
|
|
q[0] = 1.;
|
|
q[1] = 0.2;
|
|
q[2] = -2.3;
|
|
q[3] = -2.3;
|
|
q[4] = 0.03;
|
|
q[5] = -0.23;
|
|
|
|
UpdateKinematics (*model, q, qdot, qddot);
|
|
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);
|
|
}
|
|
|
|
TEST ( Model2DoFJoint ) {
|
|
// the standard modeling using a null body
|
|
Body null_body;
|
|
Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.));
|
|
|
|
Model model_std;
|
|
model_std.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
|
|
model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body);
|
|
|
|
// using a model with a 2 DoF joint
|
|
Joint joint_rot_zx (
|
|
SpatialVector (0., 0., 1., 0., 0., 0.),
|
|
SpatialVector (1., 0., 0., 0., 0., 0.)
|
|
);
|
|
|
|
Model model_2;
|
|
model_2.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zx, body);
|
|
|
|
VectorNd Q = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd QDot = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd Tau = VectorNd::Zero(model_std.dof_count);
|
|
|
|
VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
|
|
|
|
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);
|
|
}
|
|
|
|
TEST ( Model3DoFJoint ) {
|
|
// the standard modeling using a null body
|
|
Body null_body;
|
|
Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.));
|
|
Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.));
|
|
|
|
Model model_std;
|
|
model_std.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
unsigned int body_id;
|
|
|
|
// in total we add two bodies to make sure that the transformations are
|
|
// correct.
|
|
model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
|
|
model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
|
|
body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body);
|
|
|
|
model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
|
|
model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
|
|
body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body);
|
|
|
|
// using a model with a 2 DoF joint
|
|
Joint joint_rot_zyx (
|
|
SpatialVector (0., 0., 1., 0., 0., 0.),
|
|
SpatialVector (0., 1., 0., 0., 0., 0.),
|
|
SpatialVector (1., 0., 0., 0., 0., 0.)
|
|
);
|
|
|
|
Model model_2;
|
|
model_2.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
// in total we add two bodies to make sure that the transformations are
|
|
// correct.
|
|
body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
|
|
body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
|
|
|
|
VectorNd Q = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd QDot = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd Tau = VectorNd::Zero(model_std.dof_count);
|
|
|
|
VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
|
|
|
|
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);
|
|
}
|
|
|
|
TEST ( Model6DoFJoint ) {
|
|
// the standard modeling using a null body
|
|
Body null_body;
|
|
Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
Joint joint_rot_y ( SpatialVector (0., 1., 0., 0., 0., 0.));
|
|
Joint joint_rot_x ( SpatialVector (1., 0., 0., 0., 0., 0.));
|
|
|
|
Model model_std;
|
|
model_std.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
unsigned int body_id;
|
|
|
|
Joint joint_floating_base (
|
|
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.)
|
|
);
|
|
body_id = model_std.AddBody (0, SpatialTransform(), joint_floating_base, body);
|
|
|
|
model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
|
|
model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
|
|
body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body);
|
|
|
|
// using a model with a 2 DoF joint
|
|
Joint joint_rot_zyx (
|
|
SpatialVector (0., 0., 1., 0., 0., 0.),
|
|
SpatialVector (0., 1., 0., 0., 0., 0.),
|
|
SpatialVector (1., 0., 0., 0., 0., 0.)
|
|
);
|
|
|
|
Model model_2;
|
|
model_2.gravity = Vector3d (0., -9.81, 0.);
|
|
|
|
// in total we add two bodies to make sure that the transformations are
|
|
// correct.
|
|
body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_floating_base, body);
|
|
body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
|
|
|
|
VectorNd Q = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd QDot = VectorNd::Zero(model_std.dof_count);
|
|
VectorNd Tau = VectorNd::Zero(model_std.dof_count);
|
|
|
|
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);
|
|
|
|
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);
|
|
}
|
|
|
|
TEST ( ModelFixedJointQueryBodyId ) {
|
|
// the standard modeling using a null body
|
|
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.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
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"));
|
|
}
|
|
|
|
/*
|
|
* Makes sure that when appending a body to a fixed joint the parent of the
|
|
* newly added parent is actually the moving body that the fixed body is
|
|
* attached to.
|
|
*/
|
|
TEST ( 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.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
unsigned int movable_body = 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");
|
|
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]);
|
|
}
|
|
|
|
// Adds a fixed body to another fixed body.
|
|
TEST ( ModelAppendFixedToFixedBody ) {
|
|
Body null_body;
|
|
|
|
double movable_mass = 1.1;
|
|
Vector3d movable_com (1., 0.4, 0.4);
|
|
|
|
double fixed_mass = 1.2;
|
|
Vector3d fixed_com (1.1, 0.5, 0.5);
|
|
|
|
Vector3d fixed_displacement (0., 1., 0.);
|
|
|
|
Body body(movable_mass, movable_com, Vector3d (1., 1., 1.));
|
|
Body fixed_body(fixed_mass, fixed_com, Vector3d (1., 1., 1.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
|
|
unsigned int fixed_body_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body");
|
|
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);
|
|
|
|
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);
|
|
|
|
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);
|
|
}
|
|
|
|
// Ensures that the transformations of the movable parent and fixed joint
|
|
// frame is in proper order
|
|
TEST ( ModelFixedJointRotationOrderTranslationRotation ) {
|
|
// the standard modeling using a null body
|
|
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.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.));
|
|
SpatialTransform rot_z = Xrotz (45. * M_PI / 180.);
|
|
|
|
model.AddBody (0, trans_x, joint_rot_z, body);
|
|
model.AppendBody (rot_z, Joint(JointTypeFixed), fixed_body, "fixed_body");
|
|
unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body);
|
|
|
|
VectorNd Q (VectorNd::Zero(model.dof_count));
|
|
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);
|
|
}
|
|
|
|
// Ensures that the transformations of the movable parent and fixed joint
|
|
// frame is in proper order
|
|
TEST ( ModelFixedJointRotationOrderRotationTranslation ) {
|
|
// the standard modeling using a null body
|
|
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.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
SpatialTransform rot_z = Xrotz (45. * M_PI / 180.);
|
|
SpatialTransform trans_x = Xtrans (Vector3d (1., 0., 0.));
|
|
|
|
model.AddBody (0, rot_z, joint_rot_z, body);
|
|
model.AppendBody (trans_x, Joint(JointTypeFixed), fixed_body, "fixed_body");
|
|
unsigned int body_after_fixed = model.AppendBody (trans_x, joint_rot_z, body);
|
|
|
|
VectorNd Q (VectorNd::Zero(model.dof_count));
|
|
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);
|
|
}
|
|
|
|
TEST ( 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.));
|
|
|
|
Model model;
|
|
|
|
Joint joint_rot_z ( SpatialVector (0., 0., 1., 0., 0., 0.));
|
|
|
|
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");
|
|
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));
|
|
}
|
|
|
|
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_FIXTURE(RotZRotZYXFixed, ModelGetParentIdFixed) {
|
|
CHECK_EQUAL (body_b_id, model->GetParentBodyId(body_fixed_id));
|
|
}
|
|
|
|
TEST_FIXTURE(RotZRotZYXFixed, 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);
|
|
}
|
|
|
|
TEST_FIXTURE(RotZRotZYXFixed, ModelGetJointFrameFixed) {
|
|
SpatialTransform transform_fixed = model->GetJointFrame (body_fixed_id);
|
|
|
|
CHECK_ARRAY_EQUAL (fixture_transform_fixed.r.data(), transform_fixed.r.data(), 3);
|
|
}
|
|
|
|
TEST_FIXTURE(RotZRotZYXFixed, 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.));
|
|
|
|
model->SetJointFrame (body_a_id, new_transform_a);
|
|
model->SetJointFrame (body_b_id, new_transform_b);
|
|
model->SetJointFrame (0, new_transform_root);
|
|
|
|
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 (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);
|
|
}
|
|
|
|
TEST (CalcBodyWorldOrientationFixedJoint) {
|
|
Model model_fixed;
|
|
Model model_movable;
|
|
|
|
Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.));
|
|
Joint joint_fixed (JointTypeFixed);
|
|
Joint joint_rot_x = (SpatialVector (1., 0., 0., 0., 0., 0.));
|
|
|
|
model_fixed.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body);
|
|
unsigned int body_id_fixed = model_fixed.AppendBody (Xroty (45 * M_PI / 180), joint_fixed, body);
|
|
|
|
model_movable.AppendBody (Xrotx (45 * M_PI / 180), joint_rot_x, body);
|
|
unsigned int body_id_movable = model_movable.AppendBody (Xroty (45 * M_PI / 180), joint_rot_x, body);
|
|
|
|
VectorNd q_fixed (VectorNd::Zero (model_fixed.q_size));
|
|
VectorNd q_movable (VectorNd::Zero (model_movable.q_size));
|
|
|
|
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);
|
|
}
|
|
|
|
TEST (TestAddFixedBodyToRoot) {
|
|
Model model;
|
|
|
|
Body body (1., Vector3d (1., 1., 1.), Vector3d (1., 1., 1.));
|
|
Joint joint_fixed (JointTypeFixed);
|
|
|
|
// Add a fixed body
|
|
unsigned int body_id_fixed = model.AppendBody (Xtrans(Vector3d (1., 0., 0.)), joint_fixed, body, "FixedBody");
|
|
|
|
// Add a second fixed body
|
|
unsigned int body_id_fixed2 = model.AppendBody (Xtrans(Vector3d (1., 0., 0.)), joint_fixed, body, "FixedBody2");
|
|
|
|
// 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());
|
|
|
|
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);
|
|
|
|
base_coords = CalcBodyToBaseCoordinates(model, q, body_id_fixed2, Vector3d::Zero());
|
|
CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3);
|
|
|
|
base_coords = CalcBodyToBaseCoordinates(model, q, movable_body, Vector3d::Zero());
|
|
CHECK_ARRAY_EQUAL(Vector3d (2., 0., 0.).data(), base_coords.data(), 3);
|
|
|
|
}
|
|
|