ldraw::color Selection::getLastColor() const
{
const ldraw::element_ref *ref = getLastRef();
if (ref)
return ref->get_color();
else
return ldraw::color(0);
}
ldraw::matrix Selection::getLastMatrix() const
{
const ldraw::element_ref *ref = getLastRef();
if (ref)
return ref->get_matrix();
else
return ldraw::matrix();
}
/**
* @function stayDogStay
* @brief these will add walk contexts to the back of ref and the new
* contexts don't have comX, comY, eX, eY however, the kstate will
* have body orientation set correctly and upper body joints
*/
void ZMPWalkGenerator::stayDogStay( size_t stay_ticks ) {
ZMPReferenceContext cur = getLastRef();
Eigen::Vector3d zmp_start( cur.pX, cur.pY, 0 );
Eigen::Quaterniond rot_f0( cur.feet[0].rotation() );
Eigen::Quaterniond rot_f1( cur.feet[1].rotation() );
Eigen::Affine3d midfoot( Eigen::Affine3d::Identity() );
midfoot.rotate( rot_f0.slerp(0.05, rot_f1) );
midfoot.translate( 0.5*( cur.feet[0].translation() + cur.feet[1].translation() ) );
Eigen::Vector3d zmp_end = midfoot * Eigen::Vector3d( zmpoff_x, zmpoff_y, 0 );
stance_t double_stance = DOUBLE_LEFT;
// TODO: get from timer!!
size_t shift_ticks = TRAJ_FREQ_HZ * min_double_support_time;
if (shift_ticks > stay_ticks) { shift_ticks = stay_ticks; }
for ( size_t i = 0; i < shift_ticks; ++i ) {
double u = double(i) / double(shift_ticks - 1);
double c = sigmoid(u);
cur.stance = double_stance;
Eigen::Vector3d cur_zmp = zmp_start + (zmp_end - zmp_start) * c;
cur.pX = cur_zmp.x();
cur.pY = cur_zmp.y();
ref.push_back(cur);
}
for ( size_t i = shift_ticks; i < stay_ticks; ++i ) {
ref.push_back(cur);
}
}
/**
* @function: stayDogStay(size_t stay_ticks)
* @brief: these will add walk contexts to the back of ref and the new
* contexts don't have comX, comY, eX, eY however, the kstate will
* have body orientation set correctly and upper body joints
* @return: void
*/
void ZMPWalkGenerator::stayDogStay( size_t stay_ticks ) {
ZMPReferenceContext cur = getLastRef();
vec3 zmp_start(cur.pX, cur.pY, 0);
Transform3 midfoot( quat::slerp(cur.feet[0].rotation(),
cur.feet[1].rotation(),
0.05),
0.5 * (cur.feet[0].translation() +
cur.feet[1].translation() ) );
vec3 zmp_end = midfoot * vec3(zmpoff_x, zmpoff_y, 0);
stance_t double_stance = DOUBLE_LEFT;
// TODO: get from timer!!
size_t shift_ticks = TRAJ_FREQ_HZ * min_double_support_time;
if (shift_ticks > stay_ticks) { shift_ticks = stay_ticks; }
for (size_t i=0; i<shift_ticks; ++i) {
double u = double(i) / double(shift_ticks - 1);
double c = sigmoid(u);
cur.stance = double_stance;
vec3 cur_zmp = zmp_start + (zmp_end - zmp_start) * c;
cur.pX = cur_zmp.x();
cur.pY = cur_zmp.y();
ref.push_back(cur);
}
for (size_t i=shift_ticks; i<stay_ticks; ++i) {
ref.push_back(cur);
}
}
/**
* @function addFootstep
* @brief What do you think? Add a footstep :D
*/
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
Eigen::Quaterniond start_q( start_context.feet[swing_foot].rotation() );
Eigen::Quaterniond fp_q( fp.transform.rotation() );
Eigen::Quaterniond body_rot_end = start_q.slerp( 0.5, fp_q );
// figure out the start and end positions of the zmp
Eigen::Vector3d zmp_end = start_context.feet[stance_foot] * Eigen::Vector3d( zmpoff_x, zmpoff_y, 0 );
Eigen::Vector3d zmp_start = Eigen::Vector3d( start_context.pX, start_context.pY, 0 );
std::cout << "zmp_end: "<< zmp_end.transpose();
std::cout << " zmp_start: "<< zmp_start.transpose() << std::endl;
// 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.
// ANA'S COMMENT: I THINK IT SHOULD BE - INSTEAD OF + (WE WANT DIST_THETA)
Eigen::Vector3d rotation_intermediate =
fp.transform.rotation() * Eigen::Vector3d(1.0, 0.0, 0.0) +
start_context.feet[swing_foot].rotation() * Eigen::Vector3d(1.0, 0.0, 0.0);
double dist_theta = atan2(rotation_intermediate.y(), rotation_intermediate.x()); // hooray for bad code!
//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 interpolate things like desired ZMP and body
// rotation. we run the sigmoid across both double and single
// 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;
Eigen::Vector3d cur_zmp = zmp_start + (zmp_end - zmp_start) * c;
cur_context.pX = cur_zmp.x();
cur_context.pY = cur_zmp.y();
cur_context.state.body_rot = (start_context.state.body_rot).slerp( cc, body_rot_end );
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);
Eigen::Quaterniond foot_start_rot( start_context.feet[swing_foot].rotation() );
Eigen::Quaterniond 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;
cur_context.state.body_rot = (start_context.state.body_rot).slerp( cc, body_rot_end );
Eigen::Quaterniond fs_rot( foot_start_rot );
Eigen::Quaterniond fe_rot( foot_end_rot );
// According to a post - linear access the rotation matrix! for an Affine3d
cur_context.feet[swing_foot].linear() = (fs_rot.slerp( ru, fe_rot )).toRotationMatrix();
cur_context.feet[swing_foot].translation() = Eigen::Vector3d(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;
}
}
/**
* @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;
}
}
