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