Пример #1
0
void Walker::executeTimeStep( Hubo_Control &hubo, zmp_traj_element_t &prevElem,
            zmp_traj_element_t &currentElem, 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();
}
Пример #2
0
void Walker::executeTimeStep( Hubo_Control &hubo, zmp_traj_element_t &prevElem,
            zmp_traj_element_t &currentElem, 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();
}
Пример #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;
}