int main( int argc, char **argv )
{
    Hubo_Tech hubo;
    
    std::vector<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
    }
}
int main(int argc, char **argv)
{
    // Create Hubo_Tech object
    Hubo_Tech hubo;
    // Create Fastrak object
    Fastrak fastrak;
//    ofstream myfile;
//    myfile.open("ik-output.txt");
    // Initialize fastrak by opening fastrak ach channel
    fastrak.initFastrak();
    // Set fastrak scale to 1:1
    fastrak.setFastrakScale( 1.0 );

    hubo.setJointSpeedMax( RWP, 3.0 );
    hubo.setJointSpeedMax( LWP, 3.0 );

    // Local Variables
    Vector6d q, current;
    Eigen::Vector3d trans;
    Eigen::Quaterniond quat;
    double y=0, offset, dt, ptime=hubo.getTime();
    int i=0, imax=4; // loop count variable

    // Get joint angles for given y position of the drill
    hubo.HuboDrillIK( q, y );

    // Set RSR angle to -PI so his upper arm is parallel to the floor
    // in order to avoid having the drill bit hit anything
    q(2) -= M_PI/2.0;

    // Set right arm joint angles with new RSR angle and send commands
    hubo.setRightArmAngles( q, true );

    // Get current right arm joint angles 
    hubo.getRightArmAngles( current );

    // While the norm of the right arm angles is greater than 0.075
    // keep waiting for arm to get to desired position
    while( (current-q).norm() > 0.075 )
    {
        hubo.update(); // Get latest data from ach channels
        hubo.getRightArmAngles( current ); // Get current right arm joint angles
    }

    // Set RSR angle back to initial angle
    q(2) += M_PI/2.0;

    // Set right arm joint angles with new RSR angle and send commands
    hubo.setRightArmAngles( q, true );

    // Read fastrak sensor pose for sensor 1
    fastrak.getPose( trans, quat, 1, true );

    // Define initial y position to be the offset,
    // ie. take everything relative to that position
    //
    offset = trans(1);

    // If this daemon hasn't quit...
    while(!daemon_sig_quit)
    {
        // Get latest data from ach channels
        hubo.update();
        // Take latest time minus old time to see if a new time was received
        dt = hubo.getTime() - ptime;
        // Get current time in seconds
        ptime = hubo.getTime();

        // If new frame is received from hubo.update()
        if(dt>0)
        {
            // Increment i and reset it to 0 if it exceeds the max
            i++; if(i>imax) i=0;

            // Read Fastrak sensors
            fastrak.getPose( trans, quat, 1, true );

            // Set relative y-position
            y = trans(1) - offset;

            // Get joint angles for given y position of the drill
            hubo.HuboDrillIK( q, y );

            // Set joint angles
            hubo.setRightArmAngles( q );

            // Send commands to control daemon
            hubo.sendControls();

            // Print out useful info
            if( i==imax )
            {
                std::cout << "RH Mx" << hubo.getRightHandMx()
                          << "RH My" << hubo.getRightHandMy()
                          << "RH Angles:" << q.transpose()
                          << std::endl;
            }
        }
    }
}
Example #3
0
int main(int argc, char **argv)
{
//    hubo_plus hubo("balance-test");
    /* Instantiate Hubo_Tech object from constructor */
    Hubo_Tech hubo;
//    std::ofstream myfile;
//    myfile.open("balance-output.txt");
    Fastrak fastrak;

    /* Initialize Fastrak */
    fastrak.initFastrak();

    /* Set Left Leg Joint Nominal Accelerations */
    hubo.setJointNominalAcceleration( LKN, 0.6 );
    hubo.setJointNominalAcceleration( LHP, 0.6 );
    hubo.setJointNominalAcceleration( LHR, 0.6 );
    hubo.setJointNominalAcceleration( LAR, 0.6 );
    hubo.setJointNominalAcceleration( LAP, 0.6 );

    /* Set Right Leg Joint Nominal Accelerations */
    hubo.setJointNominalAcceleration( RKN, 0.6 );
    hubo.setJointNominalAcceleration( RHP, 0.6 );
    hubo.setJointNominalAcceleration( RHR, 0.6 );
    hubo.setJointNominalAcceleration( RAP, 0.6 );
    hubo.setJointNominalAcceleration( RAR, 0.6 );

    /* Set max joint angles for both leg hip pitch joints */
    hubo.setJointAngleMax( RHP, 0 );
    hubo.setJointAngleMax( LHP, 0 );

    /* Local Variables */
    double L1 = 2*0.3002;
    double L2 = 0.28947 + 0.0795;
    double height = 0;
    Eigen::Vector3d fastrakOrigin;
    Eigen::Vector3d trans;
    Eigen::Quaterniond quat;

    double leftP=0, leftR=0, rightP=0, rightR=0;
    double knee, kneeVel, kneeAngleError;

    double compRollGain = 0.0015;
    double compPitchGain = 0.0015;
    double pitchAngleGain = 0.5*M_PI/180.0;
    double pitchRotVelGain = 0.0*M_PI/180;
    double rollAngleGain = 0.3*M_PI/180;
    double rollRotVelGain = 0.0*M_PI/180;
    double integralGain = 0; //0.05*M_PI/180;
    double kneeVelGain = 0.2;
    double ptime, dt;
    int i=0, imax=40;

    /* Get latest data from the ach channels */
    hubo.update();

    /* Get current time */
    ptime = hubo.getTime();

    /* Read Fastrak sensor to get sensor origin */
    fastrak.getPose(fastrakOrigin, quat, 1, true);

    /* Print out sensor origin location */
//    std::cout << fastrakOrigin.transpose() << std::endl;

    /* Print headers for output data */
//    fprintf(stdout, "Time,msgID,Height,KneeVel,KneeAngle\n");
//    while(!daemon_sig_quit)
    while(true)
    {
        /* Get latest data from the ach channels */
        hubo.update();

        dt = hubo.getTime() - ptime;
        ptime = hubo.getTime();
        if( dt > 0 )
//        if( true )//dt > 0 )
        {
            /* Reset i to 0 when it reaches max value */
            i++;
            if(i>imax) i=0;

            /* Read Fastrak to get translation and quaternion to sensor */
            fastrak.getPose( trans, quat, 1, true ); //Using sensor 1
            /* Print out location of fastrak sensor */
//            std::cout << trans.transpose() << std::endl;

            /* Make translation relative to initial sensor location */
            trans -= fastrakOrigin;

            /* Set height equal to the Fastrak z-position plus shoulder height of Hubo */
            height = trans(2) + L1 + L2;

            /* Keep height maximum at shoulder height */
            if( height-L2 > L1 )
                height = L1+L2;
            /* Set minimum height to 0.2 meters above waist */
            else if( height-L2 < 0.25 )
                height = L1 + 0.2; // TODO: Check if this is a good value

            /* Calculate knee angle required for calculated height value */
            knee = acos( (height-L2)/L1 )*2;

            /* Set knee angle error to (desired angle - current angle) */
            kneeAngleError = knee - hubo.getJointAngle( RKN );

            /* Set knee velocity to knee angle error times a gain */
            kneeVel = kneeVelGain*kneeAngleError;

            /* Calculate ankle roll and pitch velocities by resisting IMU angle, complying with
             * the moment about the ankle (each with gains) and set the pitch velocities to
             * negative half the knee velocities
             */
            leftP = pitchAngleGain*hubo.getAngleY() - compPitchGain*hubo.getLeftFootMy() + (-kneeVel/2);
            leftR = rollAngleGain*hubo.getAngleX() - compRollGain*hubo.getLeftFootMx();
            rightP = pitchAngleGain*hubo.getAngleY() - compPitchGain*hubo.getRightFootMy() + (-kneeVel/2);
            rightR = rollAngleGain*hubo.getAngleX() - compRollGain*hubo.getRightFootMx();

            /* Set joint velocities */
            hubo.setJointVelocity( RAP, rightP );
            hubo.setJointVelocity( RAR, rightR );
            hubo.setJointVelocity( LAP, leftP );
            hubo.setJointVelocity( LAR, leftR );

            hubo.setJointVelocity( RKN, kneeVel );
            hubo.setJointVelocity( RHP, -kneeVel/2.0 );
            hubo.setJointVelocity( LKN, kneeVel );
            hubo.setJointVelocity( LHP, -kneeVel/2.0 );

            /* Send commands to the control daemon */
            hubo.sendControls();

//            if( i==imax )
//                fprintf(stdout, "%f,%d,%f,%f\n", ptime, hubo.msgID, height, kneeVel, hubo.getJointAngle(RKN));
            if( i==imax )
            {
                std::cout
                        << "Trans: " << trans.transpose()
//                        << "\nRotatCol1: " << quat.matrix().row(0)
//                        << "\nRotatCol2: " << quat.matrix().row(1)
//                        << "\nRotatCol3: " << quat.matrix().row(2)
                        << std::endl;
                //<< "\nQuat:" << quat.w() << "\t" << quat.x() << "\t" << quat.y() << "\t" << quat.z()
            }
        }

    }
//    myfile.close();
}