void Walker::straightenBack( Hubo_Control &hubo, zmp_traj_element_t &elem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
if( elem.stance == SINGLE_LEFT )
{
state.ankle_pitch_resistance[LEFT] += dt*gains.straightening_pitch_gain[LEFT]
*( hubo.getAngleY() );
state.ankle_roll_resistance[LEFT] += dt*gains.straightening_roll_gain[LEFT]
*( hubo.getAngleX() );
}
if( elem.stance == SINGLE_RIGHT )
{
state.ankle_pitch_resistance[RIGHT] += dt*gains.straightening_pitch_gain[RIGHT]
*( hubo.getAngleY() );
state.ankle_roll_resistance[RIGHT] += dt*gains.straightening_roll_gain[RIGHT]
*( hubo.getAngleX() );
}
elem.angles[RAR] += state.ankle_roll_resistance[RIGHT];
elem.angles[RAP] += state.ankle_pitch_resistance[RIGHT];
elem.angles[LAR] += state.ankle_roll_resistance[LEFT];
elem.angles[LAP] += state.ankle_pitch_resistance[LEFT];
}
int main( int argc, char **argv )
{
Hubo_Control hubo;
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > angles(5); // This declares "angles" as a dynamic array of Vector6ds with a starting array length of 5
angles[0] << 0.0556916, 0.577126, 0.0816814, -0.492327, 0, 0;
angles[1] << -1.07878, 0.408266, -0.477742, -0.665062, 0, 0;
angles[2] << -1.17367, -0.0540511, -0.772141, -0.503859, 0, 0;
angles[3] << -0.518417, 0.172191, -0.566084, -0.0727671, 0, 0;
angles[4] << 0, 0, 0, 0, 0, 0;
// Obviously we could easily set up some code which reads in these values out of a pre-recorded file
Vector6d currentPosition; // This Vector6d will be used to track our current angle configuration
double tol = 0.075; // This will be the allowed tolerance before moving to the next point
int traj = 0; // This will keep track of our current target on the trajectory
while( true )
{
hubo.update(); // This grabs the latest state information
hubo.getLeftArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
// If you are unfamiliar with "passing by reference", let me know and I can explain the concept. It's a feature of C++ but not C
if( ( currentPosition-angles[traj] ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
traj++;
if( traj > 4 )
traj = 0;
}
hubo.setLeftArmAngles( angles[traj] ); // Notice that the second argument is not passed in, making it default to "false"
hubo.sendControls(); // This will send off all the latest control commands over ACH
}
}
void gotoFirstPosition(double referenceData[], Hubo_Control &hubo){
double tol = 0.075; // This will be the allowed tolerance before moving to the next point
ArmVector left_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector left_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
left_arm_angles<< referenceData[LSP], referenceData[LSR], referenceData[LSY], referenceData[LEB], referenceData[LWY], referenceData[LWP], referenceData[LWR], 0,0,0;
right_arm_angles<< referenceData[RSP], referenceData[RSR], referenceData[RSY], referenceData[REB], referenceData[RWY], referenceData[RWP], referenceData[RWR],0,0,0;
right_leg_angles<< referenceData[RHY], referenceData[RHR], referenceData[RHP], referenceData[RKN], referenceData[RAP], referenceData[RAR],0,0,0,0;
left_leg_angles<< referenceData[LHY], referenceData[LHR], referenceData[LHP], referenceData[LKN], referenceData[LAP], referenceData[LAR],0,0,0,0;
bool left_arm_in_limit=false;
bool right_arm_in_limit=false;
bool left_leg_in_limit=false;
bool right_leg_in_limit=false;
ArmVector currentPosition; // This ArmVector will be used to track our current angle configuration
while( left_arm_in_limit && right_arm_in_limit && left_leg_in_limit && right_leg_in_limit )
{
hubo.update(true); // This grabs the latest state information
hubo.getLeftArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
// If you are unfamiliar with "passing by reference", let me know and I can explain the concept. It's a feature of C++ but not C
if( ( currentPosition-left_arm_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
left_arm_in_limit=true;
}
hubo.getRightArmAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-right_arm_angles).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
right_arm_in_limit=true;
}
hubo.getLeftLegAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-left_leg_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
left_leg_in_limit=true;
}
hubo.getRightLegAngleStates( currentPosition ); // This will fill in the values of "currentPosition" by passing it in by reference
if( ( currentPosition-right_leg_angles ).norm() < tol ) // The class function .norm() is a feature of the Eigen libraries. Eigen has many other extremely useful functions like this.
{
right_leg_in_limit=true;
}
hubo.setLeftArmAngles( left_arm_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setRightArmAngles( right_arm_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setLeftLegAngles( left_leg_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.setRightLegAngles( right_leg_angles); // Notice that the second argument is not passed in, making it default to "false"
hubo.sendControls(); // This will send off all the latest control commands over ACH
}
}
int main( int argc, char ** argv) {
Hubo_Control hubo;
Vector6d q;
q << -20.0/180.0*M_PI, 0, 0, -M_PI/2+20.0/180.0*M_PI, -0.3, 0;
hubo.setRightArmAngles( q, true);
}
void Walker::flattenFoot( Hubo_Control &hubo, zmp_traj_element_t &elem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
state.ankle_roll_compliance[LEFT] -= gains.decay_gain[LEFT]*state.ankle_roll_compliance[LEFT];
state.ankle_roll_compliance[RIGHT] -= gains.decay_gain[RIGHT]*state.ankle_roll_compliance[RIGHT];
state.ankle_pitch_compliance[LEFT] -= gains.decay_gain[LEFT]*state.ankle_pitch_compliance[LEFT];
state.ankle_pitch_compliance[RIGHT] -= gains.decay_gain[RIGHT]*state.ankle_pitch_compliance[RIGHT];
if( gains.force_min_threshold[RIGHT] < hubo.getRightFootFz()
&& hubo.getRightFootFz() < gains.force_max_threshold[RIGHT] )
{
state.ankle_roll_compliance[RIGHT] += dt*gains.flattening_gain[RIGHT]
*( hubo.getRightFootMx() );
state.ankle_pitch_compliance[RIGHT] += dt*gains.flattening_gain[RIGHT]
*( hubo.getRightFootMy() );
}
if( gains.force_min_threshold[LEFT] < hubo.getLeftFootFz()
&& hubo.getLeftFootFz() < gains.force_max_threshold[LEFT] )
{
state.ankle_roll_compliance[LEFT] += dt*gains.flattening_gain[LEFT]
*( hubo.getLeftFootMx() );
state.ankle_pitch_compliance[LEFT] += dt*gains.flattening_gain[LEFT]
*( hubo.getLeftFootMy() );
}
elem.angles[RAR] += state.ankle_roll_compliance[RIGHT];
elem.angles[RAP] += state.ankle_pitch_compliance[RIGHT];
elem.angles[LAR] += state.ankle_roll_compliance[LEFT];
elem.angles[LAP] += state.ankle_pitch_compliance[LEFT];
}
int main( int argc, char **argv )
{
Hubo_Control hubo;
// Vector6d is derived from the Matrix template in the Eigen C++ library
// Vector6d is conveniently used for operations related to the arms and legs of Hubo because they are 6 DOF
Vector6d q;
// The left shift operator is a convenient way of setting the values of a Vector6d:
q << -20.0/180.0*M_PI, 0, 0, -M_PI/2+20.0/180.0*M_PI, -0.3, 0;
// This will tell the control daemon that you want the right arm joints to move to the positions specified by q:
hubo.setRightArmAngles( q, true );
// Setting the second argument to "true" makes it so the command is sent immediately instead of waiting until the end of a loop
// Alternatively, you can say hubo.setRightArmAngles( q ) and it will assume "false" for the second argument as we will see in a later example
}
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();
}
/**
* @function: main(int argc, char **argv)
* @brief: Main function that loops reading the sensors and commanding
* Hubo's arm joints based on the poses of the foots
*/
int main(int argc, char **argv)
{
// check if no arguments given, if not report usage
if (argc < 2)
{
usage(std::cerr);
return 1;
}
// command line argument variables
bool left = false; // whether to set left arm angles
bool right = false; // whether to set right arm angles
bool print = false; // whether to print output or not
bool send = true; // whether to send commands or not
int leftSensorNumberDefault = 3; // default left foot sensor number
int rightSensorNumberDefault = 4; // default right foot sensor number
int leftSensorNumber = leftSensorNumberDefault; // left foot sensor number
int rightSensorNumber = rightSensorNumberDefault; // right foot sensor number
const char *teleopDeviceName = "liberty"; // name of teleop device
// local variables
LegVector lActualAngles, lLegAnglesNext, lLegAnglesCurrent;
LegVector rActualAngles, rLegAnglesNext, rLegAnglesCurrent;
Vector3d lFootOrigin, lSensorChange, lSensorOrigin, lSensorPos;
Vector3d rFootOrigin, rSensorChange, rSensorOrigin, rSensorPos;
Eigen::Matrix3d lRotInitial, rRotInitial, lSensorRot, rSensorRot;
Eigen::Isometry3d lFootInitialPose, lFootPoseCurrent, lFootPoseDesired;
Eigen::Isometry3d rFootInitialPose, rFootPoseCurrent, rFootPoseDesired;
LegVector speeds; speeds << 0.75, 0.75, 0.75, 0.75, 0.75, 0.75;
LegVector accels; accels << 0.40, 0.40, 0.40, 0.40, 0.40, 0.40;
double initialFootHeight = 0.1;
double dt, ptime;
int counter=0, counterMax=50;
bool updateRight;
// command line long options
const struct option long_options[] =
{
{ "left", optional_argument, 0, 'l' },
{ "right", optional_argument, 0, 'r' },
{ "nosend", no_argument, 0, 'n' },
{ "device", optional_argument, 0, 'd' },
{ "verbose", no_argument, 0, 'V' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
// command line short options
const char* short_options = "l::r::nd::VH";
// command line option and option index number
int opt, option_index;
// loop through command line options and set values accordingly
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 )
{
switch (opt)
{
case 'l': left = true; if(NULL != optarg) leftSensorNumber = getSensorNumber(optarg); break;
case 'r': right = true; if(NULL != optarg) rightSensorNumber = getSensorNumber(optarg); break;
case 'n': send = false; break;
case 'd': if(NULL != optarg) teleopDeviceName = getDeviceName(optarg); break;
case 'V': print = true; break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
// check to see if there are any invalid arguments on command line
if (optind < argc)
{
std::cerr << "Error: extra arguments on command line.\n\n";
usage(std::cerr);
exit(1);
}
// make sure the sensor numbers are not the same for both feet
if(leftSensorNumber == rightSensorNumber)
{
if(left == true && right == true)
{
std::cerr << "Error!\nSensor #'s are the same.\n"
<< "Default sensor #'s are \n\tLEFT: " << leftSensorNumberDefault
<< "\n\tRIGHT: " << rightSensorNumberDefault
<< ".\nPlease choose different sensor numbers.\n\n";
usage(std::cerr);
exit(1);
}
}
Hubo_Control hubo; // Create Hubo_Control object
// Hubo_Control hubo("teleop-arms"); // Create Hubo_Control object and daemonize program
Collision_Checker collisionChecker; // Create Collision_Checker object
// Create Teleop object
Teleop teleop(teleopDeviceName); // Create Teleop object
if (left == true) // if using the left arm
{
teleop.getPose( lSensorOrigin, lRotInitial, leftSensorNumber, true ); // get initial sensor pose
hubo.setLeftLegNomSpeeds( speeds ); // Set left arm nominal joint speeds
hubo.setLeftLegNomAcc( accels ); // Set left arm nominal joint accelerations
}
if (right == true) // if using the right arm
{
if(left == true) updateRight = false; else updateRight = true;
teleop.getPose( rSensorOrigin, rRotInitial, rightSensorNumber, updateRight ); // get initial sensor pose
hubo.setRightLegNomSpeeds( speeds ); // Set right arm nominal joint speeds
hubo.setRightLegNomAcc( accels ); // Set right arm nomimal joint accelerations
}
if(send == true) // if user wants to send commands
hubo.sendControls(); // send commands to the control daemon
if(left == true)
{
hubo.getLeftLegAngles(lLegAnglesNext);
hubo.huboLegFK(lFootInitialPose, lLegAnglesNext, LEFT); // Get left foot pose
lFootOrigin = lFootInitialPose.translation(); // Set relative zero for foot location
}
if(right == true)
{
hubo.getRightLegAngles(rLegAnglesNext);
hubo.huboLegFK(rFootInitialPose, rLegAnglesNext, RIGHT); // Get right foot pose
rFootOrigin = rFootInitialPose.translation(); // Set relative zero for foot location
}
// while the daemon is running
while(!daemon_sig_quit)
{
hubo.update(); // Get latest state info from Hubo
dt = hubo.getTime() - ptime; // compute change in time
ptime = hubo.getTime(); // get current time
if(dt>0 || (send == false && print == true)); // if new data was received over ach
{
if(left == true) // if using left arm
{
hubo.getLeftLegAngles(lLegAnglesCurrent); // get left arm joint angles
hubo.huboLegFK(lFootPoseCurrent, lLegAnglesCurrent, LEFT); // get left foot pose
teleop.getPose(lSensorPos, lSensorRot, leftSensorNumber, true); // get teleop data
lSensorChange = lSensorPos - lSensorOrigin; // compute teleop relative translation
lFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix
lFootPoseDesired.translate(lSensorChange + lFootOrigin); // pretranslate relative translation
// make sure feet don't cross sagittal plane
if(lFootPoseDesired(1,3) - FOOT_WIDTH/2 < 0)
lFootPoseDesired(1,3) = FOOT_WIDTH/2;
lFootPoseDesired.rotate(lSensorRot); // add rotation to top-left of TF matrix
hubo.huboLegIK( lLegAnglesNext, lFootPoseDesired, lLegAnglesCurrent, LEFT ); // get joint angles for desired TF
hubo.setLeftLegAngles( lLegAnglesNext, false ); // set joint angles
hubo.getLeftLegAngles( lActualAngles ); // get current joint angles
}
if( right==true ) // if using right arm
{
if(left == true) updateRight = false; else updateRight = true;
hubo.getRightLegAngles(rLegAnglesCurrent); // get right arm joint angles
hubo.huboLegFK(rFootPoseCurrent, rLegAnglesCurrent, RIGHT); // get right foot pose
teleop.getPose(rSensorPos, rSensorRot, rightSensorNumber, updateRight); // get teleop data
rSensorChange = rSensorPos - rSensorOrigin; // compute teleop relative translation
rFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix
rFootPoseDesired.translate(rSensorChange + rFootOrigin); // pretranslation by relative translation
// make sure feet don't cross sagittal plane
if(rFootPoseDesired(1,3) + FOOT_WIDTH/2 > 0)
rFootPoseDesired(1,3) = -FOOT_WIDTH/2;
rFootPoseDesired.rotate(rSensorRot); // add rotation to top-left corner of TF matrix
hubo.huboLegIK( rLegAnglesNext, rFootPoseDesired, rLegAnglesCurrent, RIGHT ); // get joint angles for desired TF
hubo.setRightLegAngles( rLegAnglesNext, false ); // set joint angles
hubo.getRightLegAngles( rActualAngles ); // get current joint angles
}
if( send == true ) // if user wants to send commands the control boards
hubo.sendControls(); // send reference commands set above
if( counter>=counterMax && print==true ) // if user wants output, print output every imax cycles
{
std::cout
<< "Teleop Position Lt(m): " << lSensorChange.transpose()
<< "\nTeleop Rotation Lt: \n" << lSensorRot
<< "\nTeleop Position Rt(m): " << rSensorChange.transpose()
<< "\nTeleop Rotation Rt: \n" << rSensorRot
<< "\nLeft Leg Actual Angles (rad): " << lActualAngles.transpose()
<< "\nLeft Leg Desired Angles(rad): " << lLegAnglesNext.transpose()
<< "\nRight Leg Actual Angles (rad): " << rActualAngles.transpose()
<< "\nRight Leg Desired Angles(rad): " << rLegAnglesNext.transpose()
<< "\nRight Foot Desired Pose: \n" << rFootPoseDesired.matrix()
<< "\nLeft Foot Desired Pose: \n" << lFootPoseDesired.matrix()
<< "\nRight foot torques(N-m)(Mx,My): " << hubo.getRightFootMx() << ", " << hubo.getRightFootMy()
<< "\nLeft foot torques(N-m)(Mx,My): " << hubo.getLeftFootMx() << ", " << hubo.getLeftFootMy()
<< std::endl;
}
if(counter>=counterMax) counter=0; counter++; // reset counter if it reaches counterMax
}
}
}
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();
}
void Walker::nudgeHips( Hubo_Control &hubo, zmp_traj_element_t &elem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
bool debug = false;
double kP, kD; //!< Proportional and derivative gains
int side; //!< variable for stance leg
// Figure out if we're in single or double support stance and which leg
switch(elem.stance)
{
case SINGLE_LEFT:
side = LEFT;
kP = gains.single_support_hip_nudge_kp;
kD = gains.single_support_hip_nudge_kd;
break;
case SINGLE_RIGHT:
side = RIGHT;
kP = gains.single_support_hip_nudge_kp;
kD = gains.single_support_hip_nudge_kd;
break;
case DOUBLE_LEFT:
case DOUBLE_RIGHT:
side = 100;
kP = gains.double_support_hip_nudge_kp;
kD = gains.double_support_hip_nudge_kd;
break;
default:
return;
}
// Store leg joint angels for current trajectory timestep
std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > qPrev(2);
qPrev[LEFT](HY) = elem.angles[LHY],
qPrev[LEFT](HR) = elem.angles[LHR],
qPrev[LEFT](HP) = elem.angles[LHP],
qPrev[LEFT](KN) = elem.angles[LKN],
qPrev[LEFT](AP) = elem.angles[LAP],
qPrev[LEFT](AR) = elem.angles[LAR];
qPrev[RIGHT](HY) = elem.angles[RHY],
qPrev[RIGHT](HR) = elem.angles[RHR],
qPrev[RIGHT](HP) = elem.angles[RHP],
qPrev[RIGHT](KN) = elem.angles[RKN],
qPrev[RIGHT](AP) = elem.angles[RAP],
qPrev[RIGHT](AR) = elem.angles[RAR];
// Skew matrix for torque reaction logic
Eigen::Matrix3d skew;
skew << 0, 1, 0,
-1, 0, 0,
0, 0, 0;
// Proportional gain matrix for ankle roll and pitch
Eigen::Matrix3d shiftGainsKp;
shiftGainsKp << kP, 0, 0,
0, kP, 0,
0, 0, 0;
// Derivative gain matrix for ankle roll and pitch
Eigen::Matrix3d shiftGainsKd;
shiftGainsKd << kD, 0, 0,
0, kD, 0,
0, 0, 0;
// Get rotation matrix for each hip yaw
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > yawRot(2);
yawRot[LEFT] = Eigen::AngleAxisd(hubo.getJointAngle(LHY), Eigen::Vector3d::UnitZ()).toRotationMatrix();
yawRot[RIGHT]= Eigen::AngleAxisd(hubo.getJointAngle(RHY), Eigen::Vector3d::UnitZ()).toRotationMatrix();
// TF for body to each foot
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > footTF(2);
// New joint angles for both legs
std::vector< Vector6d, Eigen::aligned_allocator<Vector6d> > qNew(2);
// Ankle torque error XYZ (ie. Roll/Pitch/Yaw), but just setting Z to zero.
Vector3d torqueErr[2];
// Determine how much we need to nudge to hips over to account for
// error in ankle torques about the x- and y- axes.
// If Roll torque is positive (ie. leaning left) we want hips to go right (ie. negative y-direction)
// If Pitch torque is positive (ie. leaning back) we want hips to go forward (ie. positive x-direction)
// Get TFs for feet
hubo.huboLegFK( footTF[LEFT], qPrev[LEFT], LEFT );
hubo.huboLegFK( footTF[RIGHT], qPrev[RIGHT], RIGHT );
std::cout << "foot is supposedly at " << footTF[LEFT].translation().transpose() << "\n";
// Averaged torque error in ankles (roll and pitch) (yaw is always zero)
//FIXME The version below is has elem.torques negative b/c hubomz computes reaction torque at ankle
// instead of torque at F/T sensor
torqueErr[LEFT](0) = (-elem.torque[LEFT][0] - hubo.getLeftFootMx());
torqueErr[LEFT](1) = (-elem.torque[LEFT][1] - hubo.getLeftFootMy());
torqueErr[LEFT](2) = 0;
torqueErr[RIGHT](0) = (-elem.torque[RIGHT][0] - hubo.getRightFootMx());
torqueErr[RIGHT](1) = (-elem.torque[RIGHT][1] - hubo.getRightFootMy());
torqueErr[RIGHT](2) = 0;
// Feet position errors (x,y)
Vector3d instantaneousFeetOffset;
// Check if we're on the ground, if not set instantaneous feet offset
// to zero so integrated feet offset doesn't change, but we still apply it.
const double forceThreshold = 20; // Newtons
if(hubo.getLeftFootFz() + hubo.getRightFootFz() > forceThreshold)
{
std::cout << "Fzs = " << hubo.getLeftFootFz() << ", " << hubo.getRightFootFz() << "\n";
if (side != LEFT && side != RIGHT)
{
instantaneousFeetOffset = (dt*shiftGainsKp * (yawRot[LEFT]*skew*torqueErr[LEFT] + yawRot[RIGHT]*skew*torqueErr[RIGHT])/2)
- (shiftGainsKd * (yawRot[LEFT]*skew*(torqueErr[LEFT] - state.prevTorqueErr[LEFT])
+ yawRot[RIGHT]*skew*(torqueErr[RIGHT] - state.prevTorqueErr[RIGHT]))/2);
}
else
{
instantaneousFeetOffset = (dt*shiftGainsKp * yawRot[side]*skew*torqueErr[side])
- (shiftGainsKd * yawRot[side]*skew*(torqueErr[side] - state.prevTorqueErr[side]));
}
}
else
instantaneousFeetOffset.setZero();
// Decay the integratedFeetOffset
state.integratedFeetOffset -= gains.decay_gain[LEFT]*state.integratedFeetOffset;
// Add the instantaneous feet offset to the integrator
state.integratedFeetOffset += instantaneousFeetOffset;
const double integratedFeetOffsetTol = 0.06;
double n = state.integratedFeetOffset.norm();
if (n > integratedFeetOffsetTol) {
state.integratedFeetOffset *= integratedFeetOffsetTol/n;
}
// Pretranslate feet TF by integrated feet error translation vector
footTF[LEFT].pretranslate(state.integratedFeetOffset);
footTF[RIGHT].pretranslate(state.integratedFeetOffset);
// Run IK on the adjusted feet TF to get new joint angles
bool ok = true;
ok = hubo.huboLegIK(qNew[LEFT], footTF[LEFT], qPrev[LEFT], LEFT);
if(ok)
ok = hubo.huboLegIK(qNew[RIGHT], footTF[RIGHT], qPrev[RIGHT], RIGHT);
// TODO: FIXME: MZ doesn't like the above code, he will explain
hubo.huboLegFK( footTF[LEFT], qNew[LEFT], LEFT );
std::cout << "now foot is supposedly at " << footTF[LEFT].translation().transpose() << "\n";
if(debug)
{
std::cout //<< " K: " << kP
//<< " TdL: " << -elem.torque[LEFT][0] << ", " << -elem.torque[LEFT][1]
//<< " TdR: " << -elem.torque[RIGHT][0] << ", " << -elem.torque[RIGHT][1]
//<< " MyLR: " << hubo.getLeftFootMy() << ", " << hubo.getRightFootMy()
//<< " MxLR: " << hubo.getLeftFootMx() << ", " << hubo.getRightFootMx()
//<< " Te: " << torqueErr.transpose()
//<< " Fte: " << instantaneousFeetOffset.transpose()
//<< " qDfL: " << (qNew[LEFT] - qPrev[LEFT]).transpose()
<< " FeetE: " << state.integratedFeetOffset.transpose()
<< "\tqDfR: " << qNew[RIGHT].transpose()
<< "\n";
}
//ok = false;
// Set leg joint angles for current timestep of trajectory
if(ok)
{
elem.angles[LHY] = qNew[LEFT](HY);
elem.angles[LHR] = qNew[LEFT](HR);
elem.angles[LHP] = qNew[LEFT](HP);
elem.angles[LKN] = qNew[LEFT](KN);
elem.angles[LAP] = qNew[LEFT](AP);
elem.angles[LAR] = qNew[LEFT](AR);
elem.angles[RHY] = qNew[RIGHT](HY);
elem.angles[RHR] = qNew[RIGHT](HR);
elem.angles[RHP] = qNew[RIGHT](HP);
elem.angles[RKN] = qNew[RIGHT](KN);
elem.angles[RAP] = qNew[RIGHT](AP);
elem.angles[RAR] = qNew[RIGHT](AR);
}
else
std::cout << "IK Invalid\n";
// Save current force torque readings for next iteration
for(int i=0; i<2; i++)
state.prevTorqueErr[i] = torqueErr[i];
}
int main() {
printf("\n");
printf(" ******************* hubo-tech ***************** \n");
printf(" Support: Grey ([email protected] \n" );
printf(" *********************************************** \n");
printf(" Note: This is a derived version of Dan Lofaro's\n"
" hubo-console. It will be replaced in the near\n"
" future with a GUI." ); fflush(stdout);
Hubo_Control hubo; printf(" -- Hubo ready!\n"); fflush(stdout);
hubo_param H_param;
hubo_state H_state;
setJointParams( &H_param, &H_state );
setSensorDefaults( &H_param );
char *buf;
rl_attempted_completion_function = my_completion;
printf("\n");
while((buf = readline(">> hubo-ach: "))!=NULL) {
//enable auto-complete
rl_bind_key('\t',rl_complete);
printf(" ");
/* get update after every command */
hubo.update();
int tsleep = 0;
char* buf0 = getArg(buf, 0);
if (strcmp(buf0,"update")==0) {
hubo.update();
printf("--->Hubo Information Updated\n");
}
else if (strcmp(buf0,"get")==0) {
int jnt = hubo_set(buf, &H_param);
char* tmp = getArg(buf,1);
printf(">> %s = %f rad \n",tmp,hubo.getJointAngle(jnt));
}
else if (strcmp(buf0,"goto")==0) {
int jnt = hubo_set(buf, &H_param);
float f = 0.0;
char* str = getArg(buf,2);
if(sscanf(str, "%f", &f) != 0){ //It's a float.
hubo.setJointAngle( jnt, f, true );
printf(">> %s = %f rad \n",getArg(buf,1),f);
}
else {
printf(">> Bad input \n");
}
}
else if (strcmp(buf0,"beep")==0) {
int jnt = name2mot(getArg(buf, 1), &H_param);
double etime = atof(getArg(buf,2));
hubo.jointBeep( jnt, etime, true );
}
else if (strcmp(buf0,"home")==0) {
hubo.homeJoint( hubo_set(buf, &H_param), true );
printf("%s - Home \n",getArg(buf,1));
}
else if (strcmp(buf0,"homeAll")==0) {
hubo.homeAllJoints( true );
}
else if (strcmp(buf0,"reset")==0) {
int jnt = name2mot(getArg(buf, 1), &H_param);
hubo.resetJoint( jnt, true );
printf("%s - Resetting Encoder \n",getArg(buf,1));
}
else if (strcmp(buf0,"startup")==0) {
hubo.startAllSensors( true );
printf("Starting up Hubo\n");
tsleep = 2;
}
else if (strcmp(buf0,"ctrl")==0) {
int onOrOff = atof(getArg(buf,2));
if(onOrOff == 0 | onOrOff == 1) {
int jnt = name2mot(getArg(buf,1),&H_param); // set motor num
if(onOrOff==1) // 1 = on, 0 = 0ff
hubo.motorCtrlOn( jnt, true );
else if(onOrOff==0)
hubo.motorCtrlOff( jnt, true );
if(onOrOff == 0) {
printf("%s - Turning Off CTRL\n",getArg(buf,1));}
else {
printf("%s - Turning On CTRL\n",getArg(buf,1));}
}
}
else if (strcmp(buf0,"fet")==0) {
int onOrOff = atof(getArg(buf,2));
if(onOrOff == 0 | onOrOff == 1) {
int jnt = name2mot(getArg(buf,1),&H_param); // set motor num
if(onOrOff==1)
hubo.fetOn( jnt, true );
else if(onOrOff==0)
hubo.fetOff( jnt, true );
if(onOrOff == 0) {
printf("%s - Turning Off FET\n",getArg(buf,1));}
else {
printf("%s - Turning On FET\n",getArg(buf,1));}
}
}
else if (strcmp(buf0,"initialize")==0) {
int jnt = name2mot(getArg(buf,1),&H_param); // set motor num
hubo.initializeBoard( jnt, true );
printf("%s - Initialize \n",getArg(buf,1));
}
else if (strcmp(buf0,"initializeAll")==0) {
hubo.initializeAll(true);
printf("%s - Initialize All\n",getArg(buf,1));
tsleep = 2;
}
else if (strcmp(buf0,"zero")==0) {
int ft = name2sensor(getArg(buf,1), &H_param);
hubo.startSensor( (hubo_sensor_index_t)ft, true );
}
else if (strcmp(buf0,"zeracc")==0) {
int ft = name2sensor(getArg(buf,1), &H_param);
hubo.zeroTilt( (hubo_sensor_index_t)ft, true );
}
else if (strcmp(buf0,"iniSensors")==0){
printf("Nulling All Sensors\n");
hubo.startAllSensors( true );
}
/* Quit */
else if (strcmp(buf0,"quit")==0)
break;
if (buf[0]!=0)
add_history(buf);
sleep(tsleep); // sleep for tsleep sec
}
free(buf);
return 0;
}
void printFTSensorValues(Hubo_Control &hubo){
printf("Left Foot fz is %f ", hubo.getLeftFootFz());
printf("Right Foot fz is %f ", hubo.getRightFootFz());
printf("Left Hand fz is %f ", hubo.getLeftHandFz());
printf("RightHand fz is %f\n", hubo.getRightHandFz());
}
void gotoNewPosition(double referenceData[], double bufferedData[], int resample_ratio, Hubo_Control &hubo, FILE * resultFile, int line_counter, int compliance_mode){
ArmVector left_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_arm_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector left_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
ArmVector right_leg_angles; // This declares "angles" as a dynamic array of ArmVectors with a starting array length of 5
double* interpolatedData= new double[number_of_joints];
int* joint_array = new int[number_of_joints];
joint_array[0]=RHY;
joint_array[1]=RHR;
joint_array[2]=RHP;
joint_array[3]=RKN;
joint_array[4]=RAP;
joint_array[5]=RAR;
joint_array[6]=LHY;
joint_array[7]=LHR;
joint_array[8]=LHP;
joint_array[9]=LKN;
joint_array[10]=LAP;
joint_array[11]=LAR;
joint_array[12]=RSP;
joint_array[13]=RSR;
joint_array[14]=RSY;
joint_array[15]=REB;
joint_array[16]=RWY;
joint_array[17]=RWR;
joint_array[18]=RWP;
joint_array[19]=LSP;
joint_array[20]=LSR;
joint_array[21]=LSY;
joint_array[22]=LEB;
joint_array[23]=LWY;
joint_array[24]=LWR;
joint_array[25]=LWP;
joint_array[26]=NKY;
joint_array[27]=NK1;
joint_array[28]=NK2;
joint_array[29]=WST;
joint_array[30]=RF1;
joint_array[31]=RF2;
joint_array[32]=RF3;
joint_array[33]=RF4;
joint_array[34]=RF5;
joint_array[35]=LF1;
joint_array[36]=LF2;
joint_array[37]=LF3;
joint_array[38]=LF4;
joint_array[39]=LF5;
checkTrajectory(referenceData, bufferedData, line_counter);
for (int iterator=1; iterator<=resample_ratio; iterator++){
double multiplier = (double)iterator/(double)resample_ratio;
interpolatedData = interpolate_linear(referenceData, bufferedData, multiplier);
left_arm_angles<< interpolatedData[LSP], interpolatedData[LSR], interpolatedData[LSY], interpolatedData[LEB], interpolatedData[LWY], interpolatedData[LWP], interpolatedData[LWR],0,0,0;
right_arm_angles<< interpolatedData[RSP], interpolatedData[RSR], interpolatedData[RSY], interpolatedData[REB], interpolatedData[RWY], interpolatedData[RWP], interpolatedData[RWR],0,0,0;
right_leg_angles<< interpolatedData[RHY], interpolatedData[RHR], interpolatedData[RHP], interpolatedData[RKN], interpolatedData[RAP], interpolatedData[RAR],0,0,0,0;
left_leg_angles<< interpolatedData[LHY], interpolatedData[LHR], interpolatedData[LHP], interpolatedData[LKN], interpolatedData[LAP], interpolatedData[LAR],0,0,0,0;
hubo.update(true);
for (int joint=0; joint<number_of_joints; joint++){
if (compliance_mode==0){
hubo.setJointCompliance(joint_array[joint], false);
hubo.setJointAngle(joint_array[joint], interpolatedData[joint]);
}
else{
//hubo.setJointCompliance(joint_array[joint], true);
hubo.setArmAntiFriction(LEFT, true);
hubo.setArmAntiFriction(RIGHT, true);
hubo.setArmCompliance(LEFT, true); // These will turn on compliance with the default gains of hubo-ach
hubo.setArmCompliance(RIGHT, true);
//DrcHuboKin kin;
//kin.updateHubo(hubo);
//ArmVector torques; // Vector to hold expected torques due to gravity
double time, dt=0;
time = hubo.getTime();
double qlast[HUBO_JOINT_COUNT]; // Array of the previous reference commands for all the joints (needed to calculate velocity)
for(int i=0; i<HUBO_JOINT_COUNT; i++){
qlast[i] = hubo.getJointAngle(i);
}
hubo.update();
//kin.updateHubo(hubo);
dt = hubo.getTime() - time;
time = hubo.getTime();
//for( int side=0; side<2; side++){
// kin.armTorques(side, torques);
// hubo.setArmTorques(side, torques);
//}
hubo.setJointTraj(joint_array[joint], interpolatedData[joint], (interpolatedData[joint]-qlast[joint])/dt);
}
fprintf(resultFile,"%f ",interpolatedData[joint]);
}
fprintf(resultFile," \n");
fflush(resultFile);
hubo.sendControls(); // This will send off all the latest control commands over ACH
}// end of iterator loop
}
int main( int argc, char **argv )
{
Hubo_Control hubo; // This is the constructor for the hubo object
hubo.setJointAngle( REB, -M_PI/2, true ); // "true" instructs it to send the command right away. If you don't pass an argument here, the function assumes "false".
}
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;
}
void Walker::complyKnee( Hubo_Control &hubo, zmp_traj_element_t &elem,
nudge_state_t &state, balance_gains_t &gains, double dt )
{
counter++;
//-------------------------
// STANCE TYPE
//-------------------------
// Figure out if we're in single or double support stance and which leg
int side; //!< variable for stance leg
if((unsigned char*)0x8 == elem.supporting)
side = LEFT;
else if((unsigned char*)"0100" == elem.supporting)
side = RIGHT;
else
side = 100;
//-------------------------
// GAINS
//-------------------------
Eigen::Vector3d spring_gain, damping_gain;
spring_gain.setZero(); damping_gain.setZero();
spring_gain.z() = gains.spring_gain[LEFT];
damping_gain.z() = gains.damping_gain[LEFT];
//-------------------------
// COPY JOINT ANGLES
//-------------------------
// Store leg joint angels for current trajectory timestep
Vector6d qPrev[2];
qPrev[LEFT](HY) = elem.angles[LHY],
qPrev[LEFT](HR) = elem.angles[LHR],
qPrev[LEFT](HP) = elem.angles[LHP],
qPrev[LEFT](KN) = elem.angles[LKN],
qPrev[LEFT](AP) = elem.angles[LAP],
qPrev[LEFT](AR) = elem.angles[LAR];
qPrev[RIGHT](HY) = elem.angles[RHY],
qPrev[RIGHT](HR) = elem.angles[RHR],
qPrev[RIGHT](HP) = elem.angles[RHP],
qPrev[RIGHT](KN) = elem.angles[RKN],
qPrev[RIGHT](AP) = elem.angles[RAP],
qPrev[RIGHT](AR) = elem.angles[RAR];
//-------------------------
// HIP YAWS
//-------------------------
// Get rotation matrix for each hip yaw
Eigen::Matrix3d yawRot[2];
yawRot[LEFT] = Eigen::AngleAxisd(hubo.getJointAngle(LHY), Eigen::Vector3d::UnitZ()).toRotationMatrix();
yawRot[RIGHT]= Eigen::AngleAxisd(hubo.getJointAngle(RHY), Eigen::Vector3d::UnitZ()).toRotationMatrix();
//-------------------------
// FOOT TFs
//-------------------------
// Determine how much we need to nudge to hips over to account for
// error in ankle torques about the x- and y- axes.
// If Roll torque is positive (ie. leaning left) we want hips to go right (ie. negative y-direction)
// If Pitch torque is positive (ie. leaning back) we want hips to go forward (ie. positive x-direction)
// Get TFs for feet
Eigen::Isometry3d footTF[2];
hubo.huboLegFK( footTF[LEFT], qPrev[LEFT], LEFT );
hubo.huboLegFK( footTF[RIGHT], qPrev[RIGHT], RIGHT );
if(counter > 40)
std::cout << " now " << footTF[LEFT](2,3);
//-------------------------
// FORCE/TORQUE ERROR
//-------------------------
// Averaged torque error in ankles (roll and pitch) (yaw is always zero)
//FIXME The version below is has elem.torques negative b/c hubomz computes reaction torque at ankle
// instead of torque at F/T sensor
Eigen::Vector3d forceTorqueErr[2];
forceTorqueErr[LEFT](0) = (-elem.torque[LEFT][0] - hubo.getLeftFootMx());
forceTorqueErr[LEFT](1) = (-elem.torque[LEFT][1] - hubo.getLeftFootMy());
forceTorqueErr[LEFT](2) = (-elem.forces[LEFT][2] - hubo.getLeftFootFz()); //FIXME should be positive
forceTorqueErr[RIGHT](0) = (-elem.torque[RIGHT][0] - hubo.getRightFootMx());
forceTorqueErr[RIGHT](1) = (-elem.torque[RIGHT][1] - hubo.getRightFootMy());
forceTorqueErr[RIGHT](2) = (-elem.forces[RIGHT][2] - hubo.getRightFootFz()); //FIXME should be positive
// Skew matrix for torque reaction logic
Eigen::Matrix3d skew;
skew << 0, 1, 0,
-1, 0, 0,
0, 0, 1; //FIXME should be negative
skew(0,1) = 0;
skew(1,0) = 0;
//------------------------
// IMPEDANCE CONTROLLER
//------------------------
// Check if we're on the ground, if not set instantaneous feet offset
// to zero so integrated feet offset doesn't change, but we still apply it.
const double forceThreshold = 0;//20; // Newtons
if(hubo.getLeftFootFz() + hubo.getRightFootFz() > forceThreshold)
{
if(LEFT == side || RIGHT == side)
impCtrl.run(state.dFeetOffset, yawRot[side]*skew*forceTorqueErr[side], dt);
else
impCtrl.run(state.dFeetOffset, (yawRot[LEFT]*skew*forceTorqueErr[LEFT] + yawRot[RIGHT]*skew*forceTorqueErr[RIGHT])/2, dt);
}
else
{
// Don't add to the dFeetOffset
}
// Decay the dFeetOffset
// state.dFeetOffset -= gains.decay_gain[LEFT]*state.dFeetOffset;
//------------------------
// CAP BODY OFFSET
//------------------------
const double dFeetOffsetTol = 0.06;
double n = state.dFeetOffset.norm();
if (n > dFeetOffsetTol) {
state.dFeetOffset *= dFeetOffsetTol/n;
}
//------------------------
// ADJUST FEET TFs
//------------------------
// Pretranslate feet TF by integrated feet error translation vector
Eigen::Isometry3d tempFootTF[2];
tempFootTF[LEFT] = footTF[LEFT].pretranslate(state.dFeetOffset.block<3,1>(0,0));
tempFootTF[RIGHT] = footTF[RIGHT].pretranslate(state.dFeetOffset.block<3,1>(0,0));
//------------------------
// GET NEW LEG ANGLES
//------------------------
// Run IK on the adjusted feet TF to get new joint angles
bool ok = false;
// Check IK for each new foot TF. If either fails, use previous feet TF
// New joint angles for both legs
Vector6d qNew[2];
ok = hubo.huboLegIK(qNew[LEFT], tempFootTF[LEFT], qPrev[LEFT], LEFT);
if(ok)
{
ok = hubo.huboLegIK(qNew[RIGHT], tempFootTF[RIGHT], qPrev[RIGHT], RIGHT);
state.prevdFeetOffset = state.dFeetOffset;
}
else // use previous integrated feet offset to get joint angles
{
std::cout << "IK Failed in impedance controller. Using previous feet TF.\n";
// Pretranslate feet TF by integrated feet error translation vector
footTF[LEFT].pretranslate(state.prevdFeetOffset.block<3,1>(0,0));
footTF[RIGHT].pretranslate(state.prevdFeetOffset.block<3,1>(0,0));
hubo.huboLegIK(qNew[LEFT], footTF[LEFT], qPrev[LEFT], LEFT);
hubo.huboLegIK(qNew[RIGHT], footTF[RIGHT], qPrev[RIGHT], RIGHT);
}
hubo.huboLegFK( footTF[LEFT], qNew[LEFT], LEFT );
if(counter > 40)
std::cout << " aft " << footTF[LEFT](2,3);
//----------------------
// DEBUG PRINT OUT
//----------------------
if(counter > 40)
{
if(true)
{
std::cout //<< " K: " << kP
//<< " TdL: " << -elem.torque[LEFT][0] << ", " << -elem.torque[LEFT][1]
//<< " TdR: " << -elem.torque[RIGHT][0] << ", " << -elem.torque[RIGHT][1]
//<< " MyLR: " << hubo.getLeftFootMy() << ", " << hubo.getRightFootMy()
//<< " MxLR: " << hubo.getLeftFootMx() << ", " << hubo.getRightFootMx()
<< " mFz: " << hubo.getLeftFootFz()
<< " dFz: " << -elem.forces[LEFT][2]
<< " FTe: " << forceTorqueErr[LEFT].z()
//<< " Fte: " << instantaneousFeetOffset.transpose()
<< " FeetE: " << state.dFeetOffset(2)
<< " qDfL: " << (qNew[LEFT] - qPrev[LEFT]).transpose()
<< "\n";
}
}
//-----------------------
// SET JOINT ANGLES
//-----------------------
// Set leg joint angles for current timestep of trajectory
{
elem.angles[LHY] = qNew[LEFT](HY);
elem.angles[LHR] = qNew[LEFT](HR);
elem.angles[LHP] = qNew[LEFT](HP);
elem.angles[LKN] = qNew[LEFT](KN);
elem.angles[LAP] = qNew[LEFT](AP);
elem.angles[LAR] = qNew[LEFT](AR);
elem.angles[RHY] = qNew[RIGHT](HY);
elem.angles[RHR] = qNew[RIGHT](HR);
elem.angles[RHP] = qNew[RIGHT](HP);
elem.angles[RKN] = qNew[RIGHT](KN);
elem.angles[RAP] = qNew[RIGHT](AP);
elem.angles[RAR] = qNew[RIGHT](AR);
}
if(counter > 40)
counter = 0;
}
