QPLocomotionPlan::QPLocomotionPlan(RigidBodyTree & robot, const QPLocomotionPlanSettings& settings, const std::string& lcm_channel) :
robot(robot),
settings(settings),
start_time(std::numeric_limits<double>::quiet_NaN()),
pelvis_id(robot.findLinkId(settings.pelvis_name)),
foot_body_ids(createFootBodyIdMap(robot, settings.foot_names)),
knee_indices(createJointIndicesMap(robot, settings.knee_names)),
akx_indices(createJointIndicesMap(robot, settings.akx_names)),
aky_indices(createJointIndicesMap(robot, settings.aky_names)),
plan_shift(Vector3d::Zero()),
shifted_zmp_trajectory(settings.zmp_trajectory),
last_foot_shift_time(0.0)
{
for (int i = 1; i < settings.support_times.size(); i++) {
if (settings.support_times[i] < settings.support_times[i - 1])
throw std::runtime_error("support times must be increasing");
}
for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
toe_off_active[*it] = false;
knee_pd_active[*it] = false;
knee_pd_settings[*it] = QPLocomotionPlanSettings::createDefaultKneeSettings();
foot_shifts[*it] = Vector3d::Zero();
}
if (!lcm.good())
{
cerr << "ERROR: lcm is not good()" << endl;
}
}
const std::map<Side, int> QPLocomotionPlan::createFootBodyIdMap(RigidBodyTree & robot, const std::map<Side, std::string>& foot_names)
{
std::map<Side, int> foot_body_ids;
for (auto it = Side::values.begin(); it != Side::values.end(); ++it) {
foot_body_ids[*it] = robot.findLinkId(foot_names.at(*it));
}
return foot_body_ids;
}