step_traj_t step_times(vector<step_t> step_seq, GaitTimer timer) { EIGEN_V_VEC2D zmpref; // b/c cannot dynamically resize matrixxd vector<stance_t> stance; int n_steps = step_seq.size(); // number of real foot steps assert(n_steps >= 0); // first and last step are stationary steps size_t curr_ticks = 0; size_t step_ticks; size_t double_support_ticks = 0; size_t single_support_ticks = 0; stance_t s = DOUBLE_LEFT; Vector2d zmp; for(int i=0; i < n_steps; i++) { s = step_seq[i].support; // Always start step in double support phase // if(s == SINGLE_LEFT) { s = DOUBLE_LEFT; } if(s == SINGLE_RIGHT) { s = DOUBLE_RIGHT; } zmp = step_seq[i].pos; if(i == 0) { // hold starting position double_support_ticks = timer.compute_startup(); single_support_ticks = 0; } else if(i < n_steps - 1) { // real foot steps double_support_ticks = timer.compute_double(0); single_support_ticks = timer.compute_single(0,0,0); } else { // hold ending position double_support_ticks = timer.compute_shutdown(); single_support_ticks = 0; } step_ticks = double_support_ticks + single_support_ticks; // reserve space in arrays zmpref.reserve(curr_ticks + step_ticks); stance.reserve(curr_ticks + step_ticks); for(size_t j=0; j < double_support_ticks; j++) { zmpref.push_back(zmp); stance.push_back(s); } s = next_stance_table[s]; curr_ticks += double_support_ticks; for(size_t j=0; j < single_support_ticks; j++) { zmpref.push_back(zmp); stance.push_back(s); } s = next_stance_table[s]; curr_ticks += single_support_ticks; } step_traj_t traj; traj.zmpref = zmpref; traj.stance = stance; return traj; }
/** * @function addFootstep * @brief Need more explanation? */ void ZMPWalkGenerator::addFootstep(const Footprint& fp) { // initialize our timer GaitTimer timer; timer.single_support_time = min_single_support_time; timer.double_support_time = min_double_support_time; timer.startup_time = walk_startup_time; timer.startup_time = walk_shutdown_time; // grab the initial position of the zmp const ZMPReferenceContext start_context = getLastRef(); // figure out the stances for this movement stance_t double_stance = fp.is_left ? DOUBLE_RIGHT : DOUBLE_LEFT; stance_t single_stance = fp.is_left ? SINGLE_RIGHT : SINGLE_LEFT; if (step_height == 0) { single_stance = double_stance; } // figure out swing foot and stance foot for accessing int swing_foot = fp.is_left ? 0 : 1; int stance_foot = 1-swing_foot; // figure out where our body is going to end up if we put our foot there quat body_rot_end = quat::slerp(start_context.feet[swing_foot].rotation(), fp.transform.rotation(), 0.5); // figure out the start and end positions of the zmp vec3 zmp_end = start_context.feet[stance_foot] * vec3(zmpoff_x, zmpoff_y, 0); vec3 zmp_start = vec3(start_context.pX, start_context.pY, 0); // figure out how far the swing foot will be moving double dist = (fp.transform.translation() - start_context.feet[swing_foot].translation()).norm(); // turns out that extracting plane angles from 3d rotations is a bit annoying. oh well. vec3 rotation_intermediate = fp.transform.rotFwd() * vec3(1.0, 0.0, 0.0) + start_context.feet[swing_foot].rotFwd() * vec3(1.0, 0.0, 0.0); double dist_theta = atan2(rotation_intermediate.y(), rotation_intermediate.x()); // hooray for bad code! // figure out how long we spend in each stage // TODO: // dist for double support should be distance ZMP moves // no theta or step height needed for double support // // dist for single support should be distance SWING FOOT moves // that also includes change in theta, and step height size_t double_ticks = timer.compute_double(dist); size_t single_ticks = timer.compute_single(dist, dist_theta, step_height); //size_t double_ticks = TRAJ_FREQ_HZ * min_double_support_time; //size_t single_ticks = TRAJ_FREQ_HZ * min_single_support_time; for (size_t i = 0; i < double_ticks; i++) { // sigmoidally interopolate things like desired ZMP and body // rotation. we run the sigmoid across both double and isngle // support so we don't try to whip the body across for the // split-second we're in double support. double u = double(i) / double(double_ticks - 1); double c = sigmoid(u); double uu = double(i) / double(double_ticks + single_ticks - 1); double cc = sigmoid(uu); ZMPReferenceContext cur_context = start_context; cur_context.stance = double_stance; vec3 cur_zmp = zmp_start + (zmp_end - zmp_start) * c; cur_context.pX = cur_zmp.x(); cur_context.pY = cur_zmp.y(); // ANA'S COMMENT //cur_context.state.body_rot = quat::slerp(start_context.state.body_rot, // body_rot_end, cc); // END ANA'S COMMENT ref.push_back(cur_context); } double swing_foot_traj[single_ticks][3]; double swing_foot_angle[single_ticks]; // note: i'm not using the swing_foot_angle stuff cause it seemed to not work swing2Cycloids(start_context.feet[swing_foot].translation().x(), start_context.feet[swing_foot].translation().y(), start_context.feet[swing_foot].translation().z(), fp.transform.translation().x(), fp.transform.translation().y(), fp.transform.translation().z(), single_ticks, fp.is_left, step_height, swing_foot_traj, swing_foot_angle); quat foot_start_rot = start_context.feet[swing_foot].rotation(); quat foot_end_rot = fp.transform.rotation(); // we want to have a small deadband at the front and end when we rotate the foot around // so it has no rotational velocity during takeoff and landing size_t rot_dead_ticks = 0.1 * TRAJ_FREQ_HZ; if (single_ticks < 4*rot_dead_ticks) { rot_dead_ticks = single_ticks / 4; } assert(single_ticks > rot_dead_ticks * 2); size_t central_ticks = single_ticks - 2*rot_dead_ticks; for (size_t i = 0; i < single_ticks; i++) { double uu = double(i + double_ticks) / double(double_ticks + single_ticks - 1); double cc = sigmoid(uu); double ru; if (i < rot_dead_ticks) { ru = 0; } else if (i < rot_dead_ticks + central_ticks) { ru = double(i - rot_dead_ticks) / double(central_ticks - 1); } else { ru = 1; } ref.push_back(getLastRef()); ZMPReferenceContext& cur_context = ref.back(); cur_context.stance = single_stance; // ANA'S C OMMENT // cur_context.state.body_rot = quat::slerp(start_context.state.body_rot, // body_rot_end, cc); // END ANA'S COMMENT cur_context.feet[swing_foot].setRotation(quat::slerp(foot_start_rot, foot_end_rot, ru)); cur_context.feet[swing_foot].setTranslation(vec3(swing_foot_traj[i][0], swing_foot_traj[i][1], swing_foot_traj[i][2])); } // finally, update the first step variable if necessary if (first_step_index == size_t(-1)) { // we haven't taken a first step yet first_step_index = ref.size() - 1; } }