<commit_before>/**
 * @file
 * @author <NAME>
 * @author <NAME>
 * @author <NAME>
 */

#include "oru_walk.h"
#include "log_debug.h"


void oru_walk::walk()
{
    /// @attention Hardcoded parameters.
    control_sampling_time_ms = 10;
    preview_sampling_time_ms = 20;
    next_preview_len_ms = 0;
    int preview_window_size = 40;
    feedback_gain = 0.2;
    feedback_threshold = 0.008;


// WMG
    initWMG (preview_window_size);


// solver    
    if (solver != NULL)
    {
        delete solver;
    }
    /// @attention Hardcoded parameters.
    solver = new smpc::solver(
            wmg->N, // size of the preview window
            1500.0,  // Alpha
            11000.0,  // Beta
            1.0,    // Gamma
            0.01,   // regularization
            1e-7);  // tolerance


// models
    initNaoModel ();
    wmg->init_param (     
            (double) preview_sampling_time_ms / 1000, // sampling time in seconds
            nao.CoM_position[2],                      // height of the center of mass
            0.015);                // step height (for interpolation of feet movements)
    /// 0.0135 in our version
    /// @ref AldNaoPaper "0.015 in the paper"

    wmg->initABMatrices ((double) control_sampling_time_ms / 1000);
    wmg->init_state.set (nao.CoM_position[0], nao.CoM_position[1]);
    old_state = wmg->init_state;


    ORUW_LOG_OPEN;

// Connect callback to the DCM post proccess
    try
    {
        fDCMPostProcessConnection =
            getParentBroker()->getProxy("DCM")->getModule()->atPostProcess
            (boost::bind(&oru_walk::callbackEveryCycle_walk, this));
    }
    catch (const AL::ALError &e)
    {
        throw ALERROR(getName(), __FUNCTION__, 
                "Error when connecting to DCM postProccess: " + e.toString());
    }
}


void oru_walk::stopWalking()
{
    fDCMPostProcessConnection.disconnect();

    ORUW_LOG_CLOSE;
}



/**
 * @brief 
 * @attention REAL-TIME!
 */
void oru_walk::callbackEveryCycle_walk()
{
    ORUW_TIMER;
    ORUW_LOG_JOINTS(accessSensorValues, accessActuatorValues);
    ORUW_LOG_COM(nao, accessSensorValues);
    ORUW_LOG_SWING_FOOT(nao, accessSensorValues);

    correctStateAndModel ();
    //updateModelJoints ();

    solveMPCProblem ();


    // support foot and swing foot position/orientation
    double swing_foot_pos[POSITION_VECTOR_SIZE];
    double angle;
    wmg->getSwingFootPosition (
            WMG_SWING_2D_PARABOLA, 
            1,
            1,
            swing_foot_pos,
            &angle);
    nao.initPosture (
            nao.swing_foot_posture, 
            swing_foot_pos, 
            0.0,    // roll angle 
            0.0,    // pitch angle
            angle); // yaw angle


    // position of CoM
    /// @attention hCoM is constant!
    nao.setCoM(wmg->init_state.x(), wmg->init_state.y(), wmg->hCoM);


    if (nao.igm_3(nao.swing_foot_posture, nao.CoM_position, nao.torso_orientation) < 0)
    {
        stopWalking();
        throw ALERROR(getName(), __FUNCTION__, "IK does not converge.");
    }

    if (nao.checkJointBounds() >= 0)
    {
        stopWalking();
        throw ALERROR(getName(), __FUNCTION__, "Joint bounds are violated.");
    }


    for (int i = 0; i < LOWER_JOINTS_NUM; i++)
    {
        walkCommands[5][i][0] = nao.q[i];
    }


    // Get time
    try
    {
        walkCommands[4][0] = dcmProxy->getTime(next_preview_len_ms);<commit_msg>New step height. Fixed bug in the timed command.<commit_after>/**
 * @file
 * @author <NAME>
 * @author <NAME>
 * @author <NAME>
 */

#include "oru_walk.h"
#include "log_debug.h"


void oru_walk::walk()
{
    /// @attention Hardcoded parameters.
    control_sampling_time_ms = 10;
    preview_sampling_time_ms = 20;
    next_preview_len_ms = 0;
    int preview_window_size = 40;
    feedback_gain = 0.2;
    feedback_threshold = 0.008;


// WMG
    initWMG (preview_window_size);


// solver    
    if (solver != NULL)
    {
        delete solver;
    }
    /// @attention Hardcoded parameters.
    solver = new smpc::solver(
            wmg->N, // size of the preview window
            1500.0,  // Alpha
            11000.0,  // Beta
            1.0,    // Gamma
            0.01,   // regularization
            1e-7);  // tolerance


// models
    initNaoModel ();
    wmg->init_param (     
            (double) preview_sampling_time_ms / 1000, // sampling time in seconds
            nao.CoM_position[2],                      // height of the center of mass
            0.02);                // step height (for interpolation of feet movements)
    /// 0.0135 in the old version of our module
    /// @ref AldNaoPaper "0.015 in the paper"
    /// 0.02 is used in the built-in module

    wmg->initABMatrices ((double) control_sampling_time_ms / 1000);
    wmg->init_state.set (nao.CoM_position[0], nao.CoM_position[1]);
    old_state = wmg->init_state;


    ORUW_LOG_OPEN;

// Connect callback to the DCM post proccess
    try
    {
        fDCMPostProcessConnection =
            getParentBroker()->getProxy("DCM")->getModule()->atPostProcess
            (boost::bind(&oru_walk::callbackEveryCycle_walk, this));
    }
    catch (const AL::ALError &e)
    {
        throw ALERROR(getName(), __FUNCTION__, 
                "Error when connecting to DCM postProccess: " + e.toString());
    }
}


void oru_walk::stopWalking()
{
    fDCMPostProcessConnection.disconnect();

    ORUW_LOG_CLOSE;
}



/**
 * @brief 
 * @attention REAL-TIME!
 */
void oru_walk::callbackEveryCycle_walk()
{
    ORUW_TIMER;
    ORUW_LOG_JOINTS(accessSensorValues, accessActuatorValues);
    ORUW_LOG_COM(nao, accessSensorValues);
    ORUW_LOG_SWING_FOOT(nao, accessSensorValues);

    correctStateAndModel ();
    //updateModelJoints ();

    solveMPCProblem ();


    // support foot and swing foot position/orientation
    double swing_foot_pos[POSITION_VECTOR_SIZE];
    double angle;
    wmg->getSwingFootPosition (
            WMG_SWING_2D_PARABOLA, 
            1,
            1,
            swing_foot_pos,
            &angle);
    nao.initPosture (
            nao.swing_foot_posture, 
            swing_foot_pos, 
            0.0,    // roll angle 
            0.0,    // pitch angle
            angle); // yaw angle


    // position of CoM
    /// @attention hCoM is constant!
    nao.setCoM(wmg->init_state.x(), wmg->init_state.y(), wmg->hCoM);


    if (nao.igm_3(nao.swing_foot_posture, nao.CoM_position, nao.torso_orientation) < 0)
    {
        stopWalking();
        throw ALERROR(getName(), __FUNCTION__, "IK does not converge.");
    }

    if (nao.checkJointBounds() >= 0)
    {
        stopWalking();
        throw ALERROR(getName(), __FUNCTION__, "Joint bounds are violated.");
    }


    for (int i = 0; i < LOWER_JOINTS_NUM; i++)
    {
        walkCommands[5][i][0] = nao.q[i];
    }


    // Get time
    try
    {
        walkCommands[4][0] = dcmProxy->getTime(control_sampling_time_ms);
{'fraction_non_alphanumeric': 0.12026693747279849, 'fraction_numerical': 0.020600609313796606, 'mean_word_length': 2.1336363636363638, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 37, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6225088', 'n_tokens_mistral': 2464, 'n_tokens_neox': 2267, 'n_words': 472}