src/walking.cpp
/**
 * @brief A control loop, that is executed in separate thread.
 * @attention REAL-TIME!
 */
@brief A control loop, that is executed in separate thread.
@attention REAL-TIME!
@brief A control loop that is executed in separate thread . @attention REAL - TIME!
void oru_walk::walkControl()
{
    oruw_timer timer(__FUNCTION__, wp.loop_time_limit_ms);
    initSolver();
    WMG wmg(wp.preview_window_size,
            wp.preview_sampling_time_ms,
            wp.step_height,
            wp.bezier_weight_1,
            wp.bezier_weight_2,
            wp.bezier_inclination_1,
            wp.bezier_inclination_2);
    wmg.T_ms[0] = wp.control_sampling_time_ms;
    wmg.T_ms[1] = wp.control_sampling_time_ms;
    smpc::state_com CoM;
    try
    {
        initWalkPattern(wmg);
        correctNextSupportPosition(wmg);
    }
    catch (...)
    {
        return;
    }
    nao.getCoM (nao.state_sensor, nao.CoM_position);
    smpc_parameters mpc(wp.preview_window_size, nao.CoM_position[2]);
    mpc.init_state.set (nao.CoM_position[0], nao.CoM_position[1]);
    jointState target_joint_state = nao.state_model;
    for (;;)
    {
        boost::unique_lock<boost::mutex> lock(walk_control_mutex);
        walk_control_condition.wait(lock);
        lock.unlock();
        timer.reset();
        ORUW_LOG_JOINTS(nao.state_sensor, target_joint_state);
        ORUW_LOG_COM(mpc, nao);
        ORUW_LOG_FEET(nao);
        try
        {
            feedbackError (mpc.init_state);
            if (solveMPCProblem (wmg, mpc))  
            {
                if (wmg.isSupportSwitchNeeded())
                {
                    correctNextSupportPosition(wmg);
                    nao.switchSupportFoot();
                }
                solver->get_state(CoM, 0);
                solveIKsendCommands (mpc, CoM, 1, wmg);
                target_joint_state = nao.state_model;
                solver->get_state(CoM, 1);
                solveIKsendCommands (mpc, CoM, 2, wmg);
            }
            else
            {
                break;
            }
            if (!timer.check()) 
            {
                halt("Time limit is violated!\n", __FUNCTION__);
            }
        }
        catch (...)
        {
            break;
        }
    }
    ORUW_LOG_STEPS(wmg);
    ORUW_LOG_CLOSE;
}
void oru_walk :: walkControl ( ) { oruw_timer timer ( __FUNCTION__ , wp . loop_time_limit_ms ) ; initSolver ( ) ; WMG wmg ( wp . preview_window_size , wp . preview_sampling_time_ms , wp . step_height , wp . bezier_weight_1 , wp . bezier_weight_2 , wp . bezier_inclination_1 , wp . bezier_inclination_2 ) ; wmg . T_ms [ 0 ] = wp . control_sampling_time_ms ; wmg . T_ms [ 1 ] = wp . control_sampling_time_ms ; smpc :: state_com CoM ; try { initWalkPattern ( wmg ) ; correctNextSupportPosition ( wmg ) ; } catch ( ... ) { return ; } nao . getCoM ( nao . state_sensor , nao . CoM_position ) ; smpc_parameters mpc ( wp . preview_window_size , nao . CoM_position [ 2 ] ) ; mpc . init_state . set ( nao . CoM_position [ 0 ] , nao . CoM_position [ 1 ] ) ; jointState target_joint_state = nao . state_model ; for ( ; ; ) { boost :: unique_lock < boost :: mutex > lock ( walk_control_mutex ) ; walk_control_condition . wait ( lock ) ; lock . unlock ( ) ; timer . reset ( ) ; ORUW_LOG_JOINTS ( nao . state_sensor , target_joint_state ) ; ORUW_LOG_COM ( mpc , nao ) ; ORUW_LOG_FEET ( nao ) ; try { feedbackError ( mpc . init_state ) ; if ( solveMPCProblem ( wmg , mpc ) ) { if ( wmg . isSupportSwitchNeeded ( ) ) { correctNextSupportPosition ( wmg ) ; nao . switchSupportFoot ( ) ; } solver -> get_state ( CoM , 0 ) ; solveIKsendCommands ( mpc , CoM , 1 , wmg ) ; target_joint_state = nao . state_model ; solver -> get_state ( CoM , 1 ) ; solveIKsendCommands ( mpc , CoM , 2 , wmg ) ; } else { break ; } if ( ! timer . check ( ) ) { halt ( " \n " , __FUNCTION__ ) ; } } catch ( ... ) { break ; } } ORUW_LOG_STEPS ( wmg ) ; ORUW_LOG_CLOSE ; }
@brief A control loop, that is executed in separate thread.
@brief A control loop that is executed in separate thread .
// steps // error in position of the swing foot // solve MPC // the old solution from is an initial guess;