void Walker::executeTimeStep( Hubo_Control &hubo, zmp_traj_element_t &prevElem,
zmp_traj_element_t ¤tElem, zmp_traj_element &nextElem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
// Make copy of zmp_traj_element so we don't effect the trajectory that's
// being recycled or it will be like recycling a changing trajectory. Not Good!
zmp_traj_element_t tempNextElem;
memcpy(&tempNextElem, &nextElem, sizeof(zmp_traj_element_t));
// int legidx[6] = { LHY, LHR, LHP, LKN, LAP, LAR };
// std::cout << "before: ";
// for (int i=0; i<6; ++i) { std::cout << tempNextElem.angles[legidx[i]] << " "; }
// std::cout << "\n";
//flattenFoot( hubo, nextElem, state, gains, dt );
//straightenBack( hubo, nextElem, state, gains, dt );
//complyKnee( hubo, tempNextElem, state, gains, dt );
//nudgeRefs( hubo, nextElem, state, dt, hkin ); //vprev, verr, dt );
// std::cout << "after: ";
// for (int i=0; i<6; ++i) { std::cout << tempNextElem.angles[legidx[i]] << " "; }
// std::cout << "\n";
// For each joint set it's position to that in the trajectory for the
// current timestep, which has been adjusted based on feedback.
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
hubo.passJointAngle( i, tempNextElem.angles[i] );
}
hubo.setJointAngleMin( LHR, currentElem.angles[RHR]-M_PI/2.0 );
hubo.setJointAngleMax( RHR, currentElem.angles[LHR]+M_PI/2.0 );
hubo.sendControls();
}
void Walker::executeTimeStep( Hubo_Control &hubo, zmp_traj_element_t &prevElem,
zmp_traj_element_t ¤tElem, zmp_traj_element &nextElem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
// Make copy of zmp_traj_element so we don't effect the trajectory that's
// being recycled or it will be like recycling a changing trajectory. Not Good!
zmp_traj_element_t tempNextElem;
memcpy(&tempNextElem, &nextElem, sizeof(zmp_traj_element_t));
// int legidx[6] = { LHY, LHR, LHP, LKN, LAP, LAR };
// std::cout << "before: ";
// for (int i=0; i<6; ++i) { std::cout << tempNextElem.angles[legidx[i]] << " "; }
// std::cout << "\n";
//flattenFoot( hubo, nextElem, state, gains, dt );
//straightenBack( hubo, nextElem, state, gains, dt );
//complyKnee( hubo, nextElem, state, gains, dt );
nudgeHips( hubo, tempNextElem, state, gains, dt );
//nudgeRefs( hubo, nextElem, state, dt, hkin ); //vprev, verr, dt );
// double vel, accel;
// std::cout << "after: ";
// for (int i=0; i<6; ++i) { std::cout << tempNextElem.angles[legidx[i]] << " "; }
// std::cout << "\n";
// For each joint set it's position to that in the trajectory for the
// current timestep, which has been adjusted based on feedback.
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
// hubo.setJointTraj( i, currentElem.angles[i] );
// hubo.setJointTraj( i, nextElem.angles[i] );
// hubo.setJointAngle( i, nextElem.angles[i] );
hubo.passJointAngle( i, tempNextElem.angles[i] + 0*bal_state.jointOffset[i] );
// Compute and set joint velocity, used by the control daemon,
// based on current and next joint angles.
//FIXME vel = (nextElem.angles[i]-currentElem.angles[i])*ZMP_TRAJ_FREQ_HZ;
//FIXME FIXME Not storing modified trajectory so this velocity doesn't make sense
// but it doesn't matter since we're using passJointAngle, which doesn't using the
// control daemon's velocity.
//vel = (nextElem.angles[i]-currentElem.angles[i])/dt;
//hubo.setJointVelocity( i, vel );
// hubo.setJointNominalSpeed(i, vel);
// hubo.setJointNominalSpeed( i, 1*
// (nextElem.angles[i]-currentElem.angles[i])*ZMP_TRAJ_FREQ_HZ/2.0 );
// (nextElem.angles[i]-currentElem.angles[i])*ZMP_TRAJ_FREQ_HZ );
// (nextElem.angles[i]-currentElem.angles[i])/dt );
// Compute and set joint acceleration, used by the control daemon,
// based on current and next joint velocities. Save new velocity in
// the state.V0 variable for use on next timestep.
//FIXME accel = (vel-state.V0[i])*ZMP_TRAJ_FREQ_HZ;
//accel = (vel-state.V0[i])/dt;
//state.V0[i] = vel;
//hubo.setJointNominalAcceleration( i, 2*accel );
// if( i == RHY || i == RHR || i == RHP || i == RKN || i == RAR || i==RAP )
// std::cout << "(" << vel << ":" << accel << ")" << "\t";
}
// std::cout << std::endl;
// hubo.setJointAngle( RSR, nextElem.angles[RSR] + hubo.getJointAngleMax(RSR) );
// hubo.setJointAngle( LSR, nextElem.angles[LSR] + hubo.getJointAngleMin(LSR) );
// hubo.setJointTraj( RSR, currentElem.angles[RSR] + hubo.getJointAngleMax(RSR) );
// hubo.setJointTraj( LSR, currentElem.angles[LSR] + hubo.getJointAngleMin(LSR) );
hubo.setJointAngleMin( LHR, currentElem.angles[RHR]-M_PI/2.0 );
hubo.setJointAngleMax( RHR, currentElem.angles[LHR]+M_PI/2.0 );
//LegVector lL, rL;
// Send all the new commands
//hubo.getLeftLegAngles(lL);
//hubo.getRightLegAngles(rL);
//std::cout << "refL: " << lL.transpose() << "\trefR: " << rL.transpose() << "\n";
hubo.sendControls();
}
int main(int argc, char **argv)
{
//=== OBJECTS ===//
// Create Hubo_Control object
Hubo_Control hubo;
redirectSigs();
// std::cout << "Daemonizing as impedanceCtrl\n";
// Hubo_Control hubo("impedanceCtrl");
//=== LOCAL VARIABLES ===//
int i=0, imax=40;
double dt, ptime;
double rHandCurrent=0;
double lHandCurrent=0;
double handCurrentStep=0.25;
bool print=true;
ptime = hubo.getTime(); // set initial time for dt(0)
std::cout << "Executing control loop ...\n";
tweak_init();
char c;
while(!daemon_sig_quit)
{
// get latest state info for Hubo
hubo.update();
dt = hubo.getTime() - ptime;
ptime = hubo.getTime();
// if new data is available...
if(dt > 0)
{
if ( read(STDIN_FILENO, &c, 1) == 1) {
switch (c) {
case 'a':
lHandCurrent += handCurrentStep;
if(lHandCurrent > 1.0) lHandCurrent = 1.0;
break;
case 'z':
lHandCurrent -= handCurrentStep;
if (lHandCurrent < -1.0) lHandCurrent = -1.0;
break;
case 's':
rHandCurrent += handCurrentStep;
if(rHandCurrent > 1.0) rHandCurrent = 1.0;
break;
case 'x':
rHandCurrent -= handCurrentStep;
if (rHandCurrent < -1.0) rHandCurrent = -1.0;
break;
}
}
hubo.passJointAngle(LF1, lHandCurrent);
hubo.passJointAngle(LF2, lHandCurrent);
hubo.passJointAngle(LF3, lHandCurrent);
hubo.passJointAngle(LF4, lHandCurrent);
hubo.passJointAngle(LF5, lHandCurrent);
hubo.passJointAngle(RF1, rHandCurrent);
hubo.passJointAngle(RF2, rHandCurrent);
hubo.passJointAngle(RF3, rHandCurrent);
hubo.passJointAngle(RF4, rHandCurrent);
hubo.passJointAngle(RF5, rHandCurrent);
hubo.sendControls();
// print output every imax cycles
if( i>=imax && print==true )
{
std::cout //<< "\033[2J"
<< "lHandCurrent: " << lHandCurrent
<< "\nrHandCurrent: " << rHandCurrent
<< "\nc: " << c
<< std::endl;
}
if(i>=imax) i=0; i++;
}
}
tty_reset(STDIN_FILENO);
return 0;
}
