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