Ejemplo n.º 1
0
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];

}
Ejemplo n.º 2
0
/**
 * @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
        }
    }
}
Ejemplo n.º 3
0
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];
}
Ejemplo n.º 4
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;
}