acados_solver_ros2  main
Acados-based NMPC controllers for ROS2 control
Functions
acados::utils Namespace Reference

Functions

template<typename Derived >
bool set_cost_field (AcadosSolver &solver, unsigned int stage, const std::string &field, Eigen::EigenBase< Derived > &value)
 Generic setter for the cost. More...
 
bool set_cost_Vx (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &Vx)
 Set the Vx matrix used for 'LINEAR_LS' cost. More...
 
bool set_cost_Vu (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &Vu)
 Set the Vu matrix used for 'LINEAR_LS' cost. More...
 
bool set_cost_Vz (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &Vz)
 Set the Vz matrix used for 'LINEAR_LS' cost. More...
 
bool set_cost_W (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &W)
 Set the W cost matrix used for 'LINEAR_LS' cost. More...
 
bool set_cost_y_ref (AcadosSolver &solver, unsigned int stage, Eigen::VectorXd &y_ref)
 Set the y_ref vector for a given stage. More...
 
template<typename Derived >
bool set_constraint_field (AcadosSolver &solver, unsigned int stage, const std::string &field, Eigen::EigenBase< Derived > &value)
 Generic setter for the constraints. More...
 
bool set_const_C (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &C)
 Set the C matrix used by polytopic constraints. More...
 
bool set_const_D (AcadosSolver &solver, unsigned int stage, Eigen::MatrixXd &D)
 Set the D matrix used by polytopic constraints. More...
 
bool set_const_g_min (AcadosSolver &solver, unsigned int stage, Eigen::VectorXd &g_min)
 Set the lower bound g_min used by polytopic constraints. More...
 
bool set_const_g_max (AcadosSolver &solver, unsigned int stage, Eigen::VectorXd &g_max)
 Set the upper bound g_max used by polytopic constraints. More...
 
bool set_const_h_min (AcadosSolver &solver, unsigned int stage, Eigen::VectorXd &h_min)
 Set the lower bound h_min used by non-linear constraints. More...
 
bool set_const_h_max (AcadosSolver &solver, unsigned int stage, Eigen::VectorXd &h_max)
 Set the upper bound h_max used by non-linear constraints. More...
 
template<typename T >
void unsafe_get_stats_field (AcadosSolver &solver, const std::string &field, T &value)
 Read a given field from the solver stats (solve must be called before...) More...
 
double get_stats_cost_value (AcadosSolver &solver)
 Retrieve the current cost value. More...
 
int get_stats_sqp_iter (AcadosSolver &solver)
 Retrieve the SQP iteration at the last solve() call. More...
 
double get_stats_cpu_time (AcadosSolver &solver)
 Retrieve the CPU time needed for the last solve() call. More...
 

Function Documentation

◆ set_cost_field()

template<typename Derived >
bool acados::utils::set_cost_field ( AcadosSolver solver,
unsigned int  stage,
const std::string &  field,
Eigen::EigenBase< Derived > &  value 
)

Generic setter for the cost.

Note
See the definition of ocp_nlp_cost_model_set in acados/interfaces/acados_c/ocp_nlp_interface.c (https://github.com/acados)
Available fields: 'Vx', 'Vu', 'Vz', 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', etc.
Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N]
fieldName of the field
valueMatrix or vector containing the data (not modified)
Returns
bool Status (true if all OK).

◆ set_cost_Vx()

bool acados::utils::set_cost_Vx ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  Vx 
)

Set the Vx matrix used for 'LINEAR_LS' cost.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
VxMatrix Vx
Returns
bool Status (true if all OK).

◆ set_cost_Vu()

bool acados::utils::set_cost_Vu ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  Vu 
)

Set the Vu matrix used for 'LINEAR_LS' cost.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
VuMatrix Vu
Returns
bool Status (true if all OK).

◆ set_cost_Vz()

bool acados::utils::set_cost_Vz ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  Vz 
)

Set the Vz matrix used for 'LINEAR_LS' cost.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
VzMatrix Vz
Returns
bool Status (true if all OK).

◆ set_cost_W()

bool acados::utils::set_cost_W ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  W 
)

Set the W cost matrix used for 'LINEAR_LS' cost.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
WMatrix W
Returns
bool Status (true if all OK).

◆ set_cost_y_ref()

bool acados::utils::set_cost_y_ref ( AcadosSolver solver,
unsigned int  stage,
Eigen::VectorXd &  y_ref 
)

Set the y_ref vector for a given stage.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
y_refReference vector
Returns
bool Status (true if all OK).

◆ set_constraint_field()

template<typename Derived >
bool acados::utils::set_constraint_field ( AcadosSolver solver,
unsigned int  stage,
const std::string &  field,
Eigen::EigenBase< Derived > &  value 
)

Generic setter for the constraints.

Note
See the definition of ocp_nlp_constraints_model_set in acados/interfaces/acados_c/ocp_nlp_interface.c (https://github.com/acados)
Available fields: 'lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'
Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N]
fieldName of the field
valueMatrix or vector containing the data (not modified, )
Returns
bool Status (true if all OK).

◆ set_const_C()

bool acados::utils::set_const_C ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  C 
)

Set the C matrix used by polytopic constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
CMatrix C
Returns
bool Status (true if all OK).

◆ set_const_D()

bool acados::utils::set_const_D ( AcadosSolver solver,
unsigned int  stage,
Eigen::MatrixXd &  D 
)

Set the D matrix used by polytopic constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
DMatrix D
Returns
bool Status (true if all OK).

◆ set_const_g_min()

bool acados::utils::set_const_g_min ( AcadosSolver solver,
unsigned int  stage,
Eigen::VectorXd &  g_min 
)

Set the lower bound g_min used by polytopic constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
g_minLower bounds on polytopic constraints.
Returns
bool Status (true if all OK).

◆ set_const_g_max()

bool acados::utils::set_const_g_max ( AcadosSolver solver,
unsigned int  stage,
Eigen::VectorXd &  g_max 
)

Set the upper bound g_max used by polytopic constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
g_maxUpper bounds on polytopic constraints.
Returns
bool Status (true if all OK).

◆ set_const_h_min()

bool acados::utils::set_const_h_min ( AcadosSolver solver,
unsigned int  stage,
Eigen::VectorXd &  h_min 
)

Set the lower bound h_min used by non-linear constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
h_minLower bounds on non-linear constraints.
Returns
bool Status (true if all OK).

◆ set_const_h_max()

bool acados::utils::set_const_h_max ( AcadosSolver solver,
unsigned int  stage,
Eigen::VectorXd &  h_max 
)

Set the upper bound h_max used by non-linear constraints.

Parameters
solverAcados solver C++ wrapper handle
stageStage in [0;N].
h_maxUpper bounds on non-linear constraints.
Returns
bool Status (true if all OK).

◆ unsafe_get_stats_field()

template<typename T >
void acados::utils::unsafe_get_stats_field ( AcadosSolver solver,
const std::string &  field,
T &  value 
)

Read a given field from the solver stats (solve must be called before...)

Warning
Dangerous method because the field type can be double or int!!! Prefer the other get_stats_<field> methods when possible or check the documentation.
Parameters
solverAcados solver C++ wrapper handle
fieldName of the field to retrieve (e.g., "time_tot", "cost_value", etc.)
Returns
double Value of the field

◆ get_stats_cost_value()

double acados::utils::get_stats_cost_value ( AcadosSolver solver)

Retrieve the current cost value.

See acados::utils::get_stats_field for implementation details.

Parameters
solverAcados solver C++ wrapper handle
Returns
double cost value

◆ get_stats_sqp_iter()

int acados::utils::get_stats_sqp_iter ( AcadosSolver solver)

Retrieve the SQP iteration at the last solve() call.

See acados::utils::get_stats_field for implementation details.

Parameters
solverAcados solver C++ wrapper handle
Returns
int SQP iteration

◆ get_stats_cpu_time()

double acados::utils::get_stats_cpu_time ( AcadosSolver solver)

Retrieve the CPU time needed for the last solve() call.

See acados::utils::get_stats_field for implementation details.

Parameters
solverAcados solver C++ wrapper handle
Returns
double CPU time in seconds