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));
}
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);
}
