GTEST_TEST(testIK, atlasIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");
Vector2d tspan;
tspan << 0, 1;
VectorXd q0 = model.getZeroConfiguration();
// The state frame of cpp model does not match with the state frame of MATLAB
// model, since the dofname_to_dofnum is different in cpp and MATLAB
q0(2) = 0.8;
Vector3d com_lb = Vector3d::Zero();
Vector3d com_ub = Vector3d::Zero();
com_lb(2) = 0.9;
com_ub(2) = 1.0;
WorldCoMConstraint com_kc(&model, com_lb, com_ub, tspan);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&com_kc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
printf("info = %d\n", info);
EXPECT_EQ(info, 1);
KinematicsCache<double> cache = model.doKinematics(q_sol);
Vector3d com = model.centerOfMass(cache);
printf("%5.6f\n%5.6f\n%5.6f\n", com(0), com(1), com(2));
EXPECT_TRUE(
CompareMatrices(com, Vector3d(0, 0, 1), 1e-6,
MatrixCompareType::absolute));
}
GTEST_TEST(testIK, iiwaIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/kuka_iiwa_arm/urdf/iiwa14.urdf");
// Create a timespan for the constraints. It's not particularly
// meaningful in this test since inverseKin() only tests a single
// point, but the constructors need something.
Vector2d tspan;
tspan << 0, 1;
// Start the robot in the zero configuration (all joints zeroed,
// pointing straight up).
VectorXd q0 = model.getZeroConfiguration();
// Constrain iiwa_link_7 (the end effector) to move 0.58 on the X
// axis and down slightly (to make room for the X axis motion).
Vector3d pos_end;
pos_end << 0.58, 0, 0.77;
const double pos_tol = 0.01;
Vector3d pos_lb = pos_end - Vector3d::Constant(pos_tol);
Vector3d pos_ub = pos_end + Vector3d::Constant(pos_tol);
const int link_7_idx = model.FindBodyIndex("iiwa_link_7");
WorldPositionConstraint wpc(&model, link_7_idx,
Vector3d(0, 0, 0), pos_lb, pos_ub, tspan);
// Constrain iiwa_joint_4 between 0.9 and 1.0.
PostureConstraint pc(&model, tspan);
drake::Vector1d joint_lb(0.9);
drake::Vector1d joint_ub(1.0);
Eigen::VectorXi joint_idx(1);
joint_idx(0) = model.findJoint("iiwa_joint_4")->get_position_start_index();
pc.setJointLimits(joint_idx, joint_lb, joint_ub);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&wpc);
constraint_array.push_back(&pc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
// Check that our constrained joint is within where we tried to constrain it.
EXPECT_GE(q_sol(joint_idx(0)), joint_lb(0));
EXPECT_LE(q_sol(joint_idx(0)), joint_ub(0));
// Check that the link we were trying to position wound up where we expected.
KinematicsCache<double> cache = model.doKinematics(q_sol);
EXPECT_TRUE(CompareMatrices(
pos_end, model.relativeTransform(cache, 0, link_7_idx).translation(),
pos_tol + 1e-6, MatrixCompareType::absolute));
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs < 1) {
mexErrMsgIdAndTxt(
"Drake:compareParsersmex:NotEnoughInputs",
"Usage compareParsersmex(model_ptr, urdf_file, floating_base_type)");
}
// first get the model_ptr back from matlab
RigidBodyTree<double>* matlab_model =
(RigidBodyTree<double>*)getDrakeMexPointer(prhs[0]);
char urdf_file[1000];
mxGetString(prhs[1], urdf_file, 1000);
char floating_base_type_str[100] = "rpy";
if (nrhs > 2) mxGetString(prhs[2], floating_base_type_str, 100);
FloatingBaseType floating_base_type = kQuaternion;
if (strcmp(floating_base_type_str, "fixed") == 0)
floating_base_type = kFixed;
else if (strcmp(floating_base_type_str, "rpy") == 0)
floating_base_type = kRollPitchYaw;
else if (strcmp(floating_base_type_str, "quat") == 0)
floating_base_type = kQuaternion;
else
mexErrMsgIdAndTxt(
"Drake:compareParsersmex:BadInputs",
"Unknown floating base type. must be 'fixed', 'rpy', or 'quat'");
auto cpp_model = std::make_unique<RigidBodyTree<double>>();
drake::parsers::PackageMap package_map;
drake::examples::AddExamplePackages(&package_map);
drake::parsers::urdf::AddModelInstanceFromUrdfFileSearchingInRosPackages(
urdf_file, package_map, floating_base_type, nullptr /* weld to frame */,
cpp_model.get());
// Compute coordinate transform between the two models (in case they are not
// identical)
MatrixXd P =
MatrixXd::Zero(cpp_model->get_num_positions(),
matlab_model->get_num_positions()); // The projection from
// the coordinates of
// the matlab_model
// to the cpp_model.
for (int i = 0; i < cpp_model->bodies.size(); ++i) {
if (cpp_model->bodies[i]->has_parent_body() &&
cpp_model->bodies[i]->getJoint().get_num_positions() > 0) {
RigidBody<double>* b = matlab_model->FindChildBodyOfJoint(
cpp_model->bodies[i]->getJoint().get_name());
if (b == nullptr) continue;
for (int j = 0; j < b->getJoint().get_num_positions(); ++j) {
P(cpp_model->bodies[i]->get_position_start_index() + j,
b->get_position_start_index() + j) = 1.0;
}
}
}
if (!P.isApprox(MatrixXd::Identity(matlab_model->get_num_positions(),
matlab_model->get_num_positions()))) {
cout << "P = \n" << P << endl;
mexErrMsgTxt("ERROR: coordinates don't match");
}
std::default_random_engine generator; // note: gets the same seed every time,
// would have to seed it manually
std::normal_distribution<double> distribution(0.0, 1.0);
for (int trial = 0; trial < 1; trial++) {
// generate random q
VectorXd matlab_q = matlab_model->getRandomConfiguration(generator);
VectorXd cpp_q(cpp_model->get_num_positions());
cpp_q.noalias() = P * matlab_q;
if ((matlab_model->get_num_positions() !=
matlab_model->get_num_velocities()) ||
(cpp_model->get_num_positions() !=
cpp_model->get_num_velocities())) {
mexErrMsgTxt(
"ERROR: num_positions != num_velocities have to generate another P "
"for "
"this case");
}
// generate random v
VectorXd matlab_v(matlab_model->get_num_velocities()),
cpp_v(cpp_model->get_num_velocities());
for (int i = 0; i < matlab_model->get_num_velocities(); i++)
matlab_v[i] = distribution(generator);
cpp_v.noalias() = P * matlab_v;
// run kinematics
KinematicsCache<double> matlab_cache =
matlab_model->doKinematics(matlab_q, matlab_v, true);
KinematicsCache<double> cpp_cache =
cpp_model->doKinematics(cpp_q, cpp_v, true);
{ // compare H, C, and B
auto matlab_H = matlab_model->massMatrix(matlab_cache);
auto cpp_H = cpp_model->massMatrix(cpp_cache);
std::string explanation;
if (!CompareMatrices(matlab_H, cpp_H, 1e-8, MatrixCompareType::absolute,
&explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: H doesn't match: " + explanation);
}
const RigidBodyTree<double>::BodyToWrenchMap no_external_wrenches;
auto matlab_C = matlab_model->dynamicsBiasTerm(matlab_cache,
no_external_wrenches);
auto cpp_C = cpp_model->dynamicsBiasTerm(cpp_cache, no_external_wrenches);
if (!CompareMatrices(matlab_C, cpp_C, 1e-8, MatrixCompareType::absolute,
&explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: C doesn't match: " + explanation);
}
if (!CompareMatrices(matlab_model->B, cpp_model->B, 1e-8,
MatrixCompareType::absolute, &explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: B doesn't match: " + explanation);
}
}
{ // compare joint limits
std::string explanation;
if (!CompareMatrices(matlab_model->joint_limit_min,
cpp_model->joint_limit_min, 1e-8,
MatrixCompareType::absolute, &explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: joint_limit_min doesn't match: " +
explanation);
}
if (!CompareMatrices(matlab_model->joint_limit_max,
cpp_model->joint_limit_max, 1e-8,
MatrixCompareType::absolute, &explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: joint_limit_max doesn't match: " +
explanation);
}
}
{ // compare position constraints
auto matlab_phi = matlab_model->positionConstraints(matlab_cache);
auto cpp_phi = cpp_model->positionConstraints(cpp_cache);
std::string explanation;
if (!CompareMatrices(matlab_phi, cpp_phi, 1e-8,
MatrixCompareType::absolute, &explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: phi doesn't match doesn't "
"match: " +
explanation);
}
auto matlab_dphi =
matlab_model->positionConstraintsJacobian(matlab_cache);
auto cpp_dphi = cpp_model->positionConstraintsJacobian(cpp_cache);
if (!CompareMatrices(matlab_dphi, cpp_dphi, 1e-8,
MatrixCompareType::absolute, &explanation)) {
throw std::runtime_error(
"Drake: CompareParserMex: ERROR: dphi doesn't match: " +
explanation);
}
}
}
}
GTEST_TEST(testIKtraj, testIKtraj) {
RigidBodyTree model(
GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");
int r_hand{};
int pelvis{};
for (int i = 0; i < static_cast<int>(model.bodies.size()); i++) {
if (!model.bodies[i]->get_name().compare(std::string("r_hand"))) {
r_hand = i;
}
if (!model.bodies[i]->get_name().compare(std::string("pelvis"))) {
pelvis = i;
}
}
VectorXd qstar = model.getZeroConfiguration();
qstar(3) = 0.8;
KinematicsCache<double> cache = model.doKinematics(qstar);
Vector3d com0 = model.centerOfMass(cache);
Vector3d r_hand_pt = Vector3d::Zero();
Vector3d rhand_pos0 = model.transformPoints(cache, r_hand_pt, r_hand, 0);
Vector2d tspan(0, 1);
int nT = 4;
double dt = tspan(1) / (nT - 1);
std::vector<double> t(nT, 0);
for (int i = 0; i < nT; i++) {
t[i] = dt * i;
}
MatrixXd q0 = qstar.replicate(1, nT);
VectorXd qdot0 = VectorXd::Zero(model.get_num_velocities());
Vector3d com_lb = com0;
com_lb(0) = std::numeric_limits<double>::quiet_NaN();
com_lb(1) = std::numeric_limits<double>::quiet_NaN();
Vector3d com_ub = com0;
com_ub(0) = std::numeric_limits<double>::quiet_NaN();
com_ub(1) = std::numeric_limits<double>::quiet_NaN();
com_ub(2) = com0(2) + 0.5;
WorldCoMConstraint com_kc(&model, com_lb, com_ub);
Vector3d rhand_pos_lb = rhand_pos0;
rhand_pos_lb(0) += 0.1;
rhand_pos_lb(1) += 0.05;
rhand_pos_lb(2) += 0.25;
Vector3d rhand_pos_ub = rhand_pos_lb;
rhand_pos_ub(2) += 0.25;
Vector2d tspan_end;
tspan_end << t[nT - 1], t[nT - 1];
WorldPositionConstraint kc_rhand(
&model, r_hand, r_hand_pt, rhand_pos_lb, rhand_pos_ub, tspan_end);
// Add a multiple time constraint which is fairly trivial to meet in
// this case.
WorldFixedBodyPoseConstraint kc_fixed_pose(&model, pelvis, tspan);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&com_kc);
constraint_array.push_back(&kc_rhand);
constraint_array.push_back(&kc_fixed_pose);
IKoptions ikoptions(&model);
MatrixXd q_sol(model.get_num_positions(), nT);
MatrixXd qdot_sol(model.get_num_velocities(), nT);
MatrixXd qddot_sol(model.get_num_positions(), nT);
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
&qddot_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
ikoptions.setFixInitialState(false);
ikoptions.setMajorIterationsLimit(500);
inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
&qddot_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
Eigen::RowVectorXd t_inbetween(5);
t_inbetween << 0.1, 0.15, 0.3, 0.4, 0.6;
ikoptions.setAdditionaltSamples(t_inbetween);
inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
&qddot_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
}
