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