269 lines
13 KiB
C++
269 lines
13 KiB
C++
#include "rbdl_tests.h"
|
|
|
|
#include <iostream>
|
|
|
|
#include "rbdl/rbdl_mathutils.h"
|
|
#include "rbdl/rbdl_utils.h"
|
|
#include "rbdl/Logging.h"
|
|
|
|
#include "rbdl/Model.h"
|
|
#include "rbdl/Kinematics.h"
|
|
|
|
#include "Human36Fixture.h"
|
|
|
|
using namespace std;
|
|
using namespace RigidBodyDynamics;
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
const double TEST_PREC = 1.0e-12;
|
|
|
|
void print_ik_set (const InverseKinematicsConstraintSet &cs) {
|
|
int label_width = 18;
|
|
cout.width (label_width);
|
|
cout << "lambda: " << cs.lambda << endl;
|
|
cout.width (label_width);
|
|
cout << "num_steps: " << cs.num_steps << endl;
|
|
cout.width (label_width);
|
|
cout << "max_steps: " << cs.max_steps << endl;
|
|
cout.width (label_width);
|
|
cout << "step_tol: " << cs.step_tol << endl;
|
|
cout.width (label_width);
|
|
cout << "constraint_tol: " << cs.constraint_tol << endl;
|
|
cout.width (label_width);
|
|
cout << "error_norm: " << cs.error_norm << endl;
|
|
}
|
|
|
|
/// Checks whether a single point in a 3-link chain can be reached
|
|
TEST_CASE_METHOD (Human36, __FILE__"_ChainSinglePointConstraint", "") {
|
|
q[HipRightRY] = 0.3;
|
|
q[HipRightRX] = 0.3;
|
|
q[HipRightRZ] = 0.3;
|
|
q[KneeRightRY] = 0.3;
|
|
q[AnkleRightRY] = 0.3;
|
|
q[AnkleRightRZ] = 0.3;
|
|
|
|
Vector3d local_point (1., 0., 0.);
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point);
|
|
|
|
q.setZero();
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point, target_position);
|
|
|
|
VectorNd qres (q);
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
if (!result) {
|
|
print_ik_set (cs);
|
|
}
|
|
|
|
REQUIRE (result);
|
|
|
|
REQUIRE_THAT (0., IsClose(cs.error_norm, TEST_PREC, TEST_PREC));
|
|
}
|
|
|
|
|
|
TEST_CASE_METHOD (Human36, __FILE__"_ManyPointConstraints", "") {
|
|
randomizeStates();
|
|
|
|
Vector3d local_point1 (1., 0., 0.);
|
|
Vector3d local_point2 (-1., 0., 0.);
|
|
Vector3d local_point3 (0., 1., 0.);
|
|
Vector3d local_point4 (1., 0., 1.);
|
|
Vector3d local_point5 (0.,0.,-1.);
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1);
|
|
Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2);
|
|
Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3);
|
|
Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4);
|
|
Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5);
|
|
|
|
q.setZero();
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddPointConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1);
|
|
cs.AddPointConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2);
|
|
cs.AddPointConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3);
|
|
cs.AddPointConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4);
|
|
cs.AddPointConstraint (body_id_emulated[BodyHead], local_point5, target_position5);
|
|
VectorNd qres (q);
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
|
|
REQUIRE (result);
|
|
|
|
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);
|
|
Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2);
|
|
Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3);
|
|
Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4);
|
|
Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5);
|
|
|
|
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_CASE_METHOD (Human36, __FILE__"_ChainSingleBodyOrientation", "") {
|
|
q[HipRightRY] = 0.3;
|
|
q[HipRightRX] = 0.3;
|
|
q[HipRightRZ] = 0.3;
|
|
q[KneeRightRY] = 0.3;
|
|
q[AnkleRightRY] = 0.3;
|
|
q[AnkleRightRZ] = 0.3;
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false);
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation);
|
|
|
|
VectorNd qres (q);
|
|
q.setZero();
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
|
|
REQUIRE (result);
|
|
}
|
|
|
|
TEST_CASE_METHOD (Human36, __FILE__"_ManyBodyOrientations", "") {
|
|
randomizeStates();
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false);
|
|
Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false);
|
|
Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false);
|
|
Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false);
|
|
Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false);
|
|
|
|
q.setZero();
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyFootRight], target_orientation1);
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyFootLeft], target_orientation2);
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyHandRight], target_orientation3);
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyHandLeft], target_orientation4);
|
|
cs.AddOrientationConstraint (body_id_emulated[BodyHead], target_orientation5);
|
|
|
|
VectorNd qres (q);
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
|
|
REQUIRE (result);
|
|
|
|
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);
|
|
Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false);
|
|
Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false);
|
|
Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false);
|
|
Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false);
|
|
|
|
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_CASE_METHOD (Human36, __FILE__"_ChainSingleBodyFullConstraint", "") {
|
|
q[HipRightRY] = 0.3;
|
|
q[HipRightRX] = 0.3;
|
|
q[HipRightRZ] = 0.3;
|
|
q[KneeRightRY] = 0.3;
|
|
q[AnkleRightRY] = 0.3;
|
|
q[AnkleRightRZ] = 0.3;
|
|
Vector3d local_point (1., 0., 0.);
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
Matrix3d target_orientation = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false);
|
|
Vector3d target_position = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point);
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddFullConstraint(body_id_emulated[BodyFootRight],local_point,target_position, target_orientation);
|
|
|
|
VectorNd qres (q);
|
|
q.setZero();
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
|
|
REQUIRE (result);
|
|
}
|
|
|
|
TEST_CASE_METHOD ( Human36, __FILE__"_ManyBodyFullConstraints", "") {
|
|
randomizeStates();
|
|
|
|
Vector3d local_point1 (1., 0., 0.);
|
|
Vector3d local_point2 (-1., 0., 0.);
|
|
Vector3d local_point3 (0., 1., 0.);
|
|
Vector3d local_point4 (1., 0., 1.);
|
|
Vector3d local_point5 (0.,0.,-1.);
|
|
|
|
UpdateKinematicsCustom (*model, &q, NULL, NULL);
|
|
|
|
Vector3d target_position1 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootRight], local_point1);
|
|
Vector3d target_position2 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyFootLeft], local_point2);
|
|
Vector3d target_position3 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandRight], local_point3);
|
|
Vector3d target_position4 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHandLeft], local_point4);
|
|
Vector3d target_position5 = CalcBodyToBaseCoordinates (*model, q, body_id_emulated[BodyHead], local_point5);
|
|
|
|
Matrix3d target_orientation1 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootRight], false);
|
|
Matrix3d target_orientation2 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyFootLeft], false);
|
|
Matrix3d target_orientation3 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandRight], false);
|
|
Matrix3d target_orientation4 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHandLeft], false);
|
|
Matrix3d target_orientation5 = CalcBodyWorldOrientation (*model, q, body_id_emulated[BodyHead], false);
|
|
|
|
InverseKinematicsConstraintSet cs;
|
|
cs.AddFullConstraint (body_id_emulated[BodyFootRight], local_point1, target_position1, target_orientation1);
|
|
cs.AddFullConstraint (body_id_emulated[BodyFootLeft], local_point2, target_position2, target_orientation2);
|
|
cs.AddFullConstraint (body_id_emulated[BodyHandRight], local_point3, target_position3, target_orientation3);
|
|
cs.AddFullConstraint (body_id_emulated[BodyHandLeft], local_point4, target_position4, target_orientation4);
|
|
cs.AddFullConstraint (body_id_emulated[BodyHead], local_point5, target_position5, target_orientation5);
|
|
cs.step_tol = 1e-12;
|
|
|
|
q.setZero();
|
|
|
|
VectorNd qres (q);
|
|
|
|
bool result = InverseKinematics (*model, q, cs, qres);
|
|
|
|
REQUIRE (result);
|
|
|
|
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);
|
|
Matrix3d result_orientation2 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyFootLeft], false);
|
|
Matrix3d result_orientation3 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandRight], false);
|
|
Matrix3d result_orientation4 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHandLeft], false);
|
|
Matrix3d result_orientation5 = CalcBodyWorldOrientation (*model, qres, body_id_emulated[BodyHead], false);
|
|
|
|
Vector3d result_position1 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootRight], local_point1);
|
|
Vector3d result_position2 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyFootLeft], local_point2);
|
|
Vector3d result_position3 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandRight], local_point3);
|
|
Vector3d result_position4 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHandLeft], local_point4);
|
|
Vector3d result_position5 = CalcBodyToBaseCoordinates (*model, qres, body_id_emulated[BodyHead], local_point5);
|
|
|
|
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));
|
|
}
|