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
}
Esempio 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
        }
    }
}
Esempio n. 3
0
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;
}