src/walking.cpp
/**
 * @brief Correct state and the model based on the sensor data.
 *
 * @param[in,out] init_state expected state
 */
@brief Correct state and the model based on the sensor data.
@param[in,out] init_state expected state
@brief Correct state and the model based on the sensor data . @param [ in out ] init_state expected state
void oru_walk::feedbackError (smpc::state_com &init_state)
{
    nao.getCoM (nao.state_sensor, nao.CoM_position);
    smpc::state_com state_error;
    state_error.set (
            init_state.x() - nao.CoM_position[0],
            init_state.y() - nao.CoM_position[1]);
    if (state_error.x() > wp.feedback_threshold)
    {
        state_error.x() -= wp.feedback_threshold;
    }
    else if (state_error.x() < -wp.feedback_threshold)
    {
        state_error.x() += wp.feedback_threshold;
    }
    else
    {
        state_error.x() = 0.0;
    }
    if (state_error.y() > wp.feedback_threshold)
    {
        state_error.y() -= wp.feedback_threshold;
    }
    else if (state_error.y() < -wp.feedback_threshold)
    {
        state_error.y() += wp.feedback_threshold;
    }
    else
    {
        state_error.y() = 0.0;
    }
    init_state.x() -= wp.feedback_gain * state_error.x();
    init_state.y() -= wp.feedback_gain * state_error.y();
}
void oru_walk :: feedbackError ( smpc :: state_com & init_state ) { nao . getCoM ( nao . state_sensor , nao . CoM_position ) ; smpc :: state_com state_error ; state_error . set ( init_state . x ( ) - nao . CoM_position [ 0 ] , init_state . y ( ) - nao . CoM_position [ 1 ] ) ; if ( state_error . x ( ) > wp . feedback_threshold ) { state_error . x ( ) -= wp . feedback_threshold ; } else if ( state_error . x ( ) < - wp . feedback_threshold ) { state_error . x ( ) += wp . feedback_threshold ; } else { state_error . x ( ) = 0.0 ; } if ( state_error . y ( ) > wp . feedback_threshold ) { state_error . y ( ) -= wp . feedback_threshold ; } else if ( state_error . y ( ) < - wp . feedback_threshold ) { state_error . y ( ) += wp . feedback_threshold ; } else { state_error . y ( ) = 0.0 ; } init_state . x ( ) -= wp . feedback_gain * state_error . x ( ) ; init_state . y ( ) -= wp . feedback_gain * state_error . y ( ) ; }
@brief Correct state and the model based on the sensor data.
@brief Correct state and the model based on the sensor data .