Thanks to visit codestin.com
Credit goes to github.com

Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 21 additions & 21 deletions acados/ocp_nlp/ocp_nlp_constraints_bgp.c
Original file line number Diff line number Diff line change
Expand Up @@ -551,7 +551,7 @@ void *ocp_nlp_constraints_bgp_model_assign(void *config, void *dims_, void *raw_
int nge = dims->nge;
int nphie = dims->nphie;

int ii;
int ii;

// struct
ocp_nlp_constraints_bgp_model *model = (ocp_nlp_constraints_bgp_model *) c_ptr;
Expand Down Expand Up @@ -584,9 +584,9 @@ void *ocp_nlp_constraints_bgp_model_assign(void *config, void *dims_, void *raw_
// h
// model->nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux = NULL;

// default initialization
for(ii=0; ii<nbue+nbxe+nge+nphie; ii++)
model->idxe[ii] = 0;
// default initialization
for(ii=0; ii<nbue+nbxe+nge+nphie; ii++)
model->idxe[ii] = 0;

// assert
assert((char *) raw_memory + ocp_nlp_constraints_bgp_model_calculate_size(config, dims) >=
Expand Down Expand Up @@ -1404,12 +1404,12 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model
// nonlinear
if (nphi > 0)
{
if(nz > 0)
{
// TODO
printf("\nerror: ocp_nlp_constraints_bgp_compute_fun: not implemented yet for nz>0\n");
exit(1);
}
if(nz > 0)
{
// TODO
printf("\nerror: ocp_nlp_constraints_bgp_compute_fun: not implemented yet for nz>0\n");
exit(1);
}

struct blasfeo_dvec_args x_in; // input x of external fun;
x_in.x = ux;
Expand All @@ -1419,7 +1419,7 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model
u_in.x = ux;
u_in.xi = 0;

// TODO tmp_z_alg !!!
// TODO tmp_z_alg !!!
struct blasfeo_dvec_args z_in; // input z of external fun;
z_in.x = memory->z_alg;
z_in.xi = 0;
Expand All @@ -1428,23 +1428,23 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model
fun_out.x = &work->tmp_ni;
fun_out.xi = nb + ng;

ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;
ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &z_in;
ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;
ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &z_in;

ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out[0] = &fun_out; // fun: nphi
ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out[0] = &fun_out; // fun: nphi

if (model->nl_constr_phi_o_r_fun == 0)
{
printf("ocp_nlp_constraints_bgp_compute_fun: nl_constr_phi_o_r_fun is not provided. Exiting.\n");
exit(1);
}
model->nl_constr_phi_o_r_fun->evaluate(model->nl_constr_phi_o_r_fun, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);
}
model->nl_constr_phi_o_r_fun->evaluate(model->nl_constr_phi_o_r_fun, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);
}

blasfeo_daxpy(nb+ng+nphi, -1.0, &work->tmp_ni, 0, &model->d, 0, &memory->fun, 0);
blasfeo_daxpy(nb+ng+nphi, -1.0, &model->d, nb+ng+nphi, &work->tmp_ni, 0, &memory->fun, nb+ng+nphi);
Expand Down
2 changes: 1 addition & 1 deletion acados/ocp_nlp/ocp_nlp_cost_common.h
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ typedef struct
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config, void *opts, const char *field, void *value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
double *(*memory_get_fun_ptr)(void *memory);
double *(*memory_get_fun_ptr)(void *memory);
struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
struct blasfeo_dvec *(*model_get_y_ref_ptr)(void *memory);
struct blasfeo_dmat *(*memory_get_W_chol_ptr)(void *memory_);
Expand Down
2 changes: 1 addition & 1 deletion acados/ocp_nlp/ocp_nlp_cost_external.h
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ typedef struct
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
double fun; ///< value of the cost function
} ocp_nlp_cost_external_memory;

//
Expand Down
2 changes: 1 addition & 1 deletion acados/ocp_nlp/ocp_nlp_cost_ls.h
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@ typedef struct
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; ///< pointer to Z in qp_in
double fun; ///< value of the cost function
double fun; ///< value of the cost function
} ocp_nlp_cost_ls_memory;

//
Expand Down
66 changes: 33 additions & 33 deletions acados/ocp_nlp/ocp_nlp_dynamics_disc.c
Original file line number Diff line number Diff line change
Expand Up @@ -454,13 +454,13 @@ void ocp_nlp_dynamics_disc_memory_get(void *config_, void *dims_, void *mem_, co

if (!strcmp(field, "time_sim") || !strcmp(field, "time_sim_ad") || !strcmp(field, "time_sim_la"))
{
double *ptr = value;
double *ptr = value;
*ptr = 0;
}
else
{
printf("\nerror: ocp_nlp_dynamics_disc_memory_get: field %s not available\n", field);
exit(1);
printf("\nerror: ocp_nlp_dynamics_disc_memory_get: field %s not available\n", field);
exit(1);
}

}
Expand Down Expand Up @@ -772,16 +772,16 @@ void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims_, void *model_,
fun_out.x = &memory->fun;
fun_out.xi = 0;

ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;
ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;

ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out[0] = &fun_out; // fun: nx1
ext_fun_type_out[0] = BLASFEO_DVEC_ARGS;
ext_fun_out[0] = &fun_out; // fun: nx1

// call external function
model->disc_dyn_fun->evaluate(model->disc_dyn_fun, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);
// call external function
model->disc_dyn_fun->evaluate(model->disc_dyn_fun, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);

// fun
blasfeo_daxpy(nx1, -1.0, ux1, nu1, &memory->fun, 0, &memory->fun, 0);
Expand Down Expand Up @@ -816,22 +816,22 @@ void ocp_nlp_dynamics_disc_compute_jac_hess_p(void *config_, void *dims_, void *
pi_in.x = memory->pi;
pi_in.xi = 0;

ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;
ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;

ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &pi_in;
ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &pi_in;

ext_fun_type_out[0] = BLASFEO_DMAT;
ext_fun_out[0] = &memory->params_jac; // jac: nx1 x np_global
ext_fun_type_out[0] = BLASFEO_DMAT;
ext_fun_out[0] = &memory->params_jac; // jac: nx1 x np_global

ext_fun_type_out[1] = BLASFEO_DMAT;
ext_fun_out[1] = &memory->params_lag_jac; // jac: nxnu x np_global
ext_fun_type_out[1] = BLASFEO_DMAT;
ext_fun_out[1] = &memory->params_lag_jac; // jac: nxnu x np_global

// call external function
model->disc_dyn_phi_jac_p_hess_xu_p->evaluate(model->disc_dyn_phi_jac_p_hess_xu_p, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);
// call external function
model->disc_dyn_phi_jac_p_hess_xu_p->evaluate(model->disc_dyn_phi_jac_p_hess_xu_p, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);

return;
}
Expand Down Expand Up @@ -939,24 +939,24 @@ void ocp_nlp_dynamics_disc_compute_adj_p(void* config_, void *dims_, void *model
pi_in.x = memory->pi;
pi_in.xi = 0;

ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;
ext_fun_type_in[0] = BLASFEO_DVEC_ARGS;
ext_fun_in[0] = &x_in;
ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
ext_fun_in[1] = &u_in;

ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &pi_in;
ext_fun_type_in[2] = BLASFEO_DVEC_ARGS;
ext_fun_in[2] = &pi_in;

ext_fun_type_out[0] = BLASFEO_DVEC;
ext_fun_out[0] = out;
ext_fun_type_out[0] = BLASFEO_DVEC;
ext_fun_out[0] = out;

// call external function
// call external function
if (model->disc_dyn_adj_p == NULL)
{
printf("ocp_nlp_dynamics_disc_compute_adj_p - model->disc_dyn_adj_p is NULL\n");
exit(1);
}
model->disc_dyn_adj_p->evaluate(model->disc_dyn_adj_p, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);
model->disc_dyn_adj_p->evaluate(model->disc_dyn_adj_p, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out);

return;
}
Expand Down
2 changes: 1 addition & 1 deletion acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c
Original file line number Diff line number Diff line change
Expand Up @@ -865,7 +865,7 @@ double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims,

merit_fun = cost_fun + dyn_fun + constr_fun;

// printf("Merit fun: %e cost: %e dyn: %e constr: %e\n", merit_fun, cost_fun, dyn_fun, constr_fun);
// printf("Merit fun: %e cost: %e dyn: %e constr: %e\n", merit_fun, cost_fun, dyn_fun, constr_fun);

return merit_fun;
}
Expand Down
34 changes: 17 additions & 17 deletions acados/ocp_nlp/ocp_nlp_reg_common.c
Original file line number Diff line number Diff line change
Expand Up @@ -98,23 +98,23 @@ ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory)

dims->N = N;

// initialize to zero by default
int ii;
// nx
for(ii=0; ii<=N; ii++)
dims->nx[ii] = 0;
// nu
for(ii=0; ii<=N; ii++)
dims->nu[ii] = 0;
// nbx
for(ii=0; ii<=N; ii++)
dims->nbx[ii] = 0;
// nbu
for(ii=0; ii<=N; ii++)
dims->nbu[ii] = 0;
// ng
for(ii=0; ii<=N; ii++)
dims->ng[ii] = 0;
// initialize to zero by default
int ii;
// nx
for(ii=0; ii<=N; ii++)
dims->nx[ii] = 0;
// nu
for(ii=0; ii<=N; ii++)
dims->nu[ii] = 0;
// nbx
for(ii=0; ii<=N; ii++)
dims->nbx[ii] = 0;
// nbu
for(ii=0; ii<=N; ii++)
dims->nbu[ii] = 0;
// ng
for(ii=0; ii<=N; ii++)
dims->ng[ii] = 0;

assert((char *) raw_memory + ocp_nlp_reg_dims_calculate_size(N) >= c_ptr);

Expand Down
6 changes: 3 additions & 3 deletions acados/ocp_nlp/ocp_nlp_reg_convexify.h
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,8 @@ typedef struct {
struct blasfeo_dmat *original_RSQrq;
struct blasfeo_dmat tmp_RSQ;

struct blasfeo_dvec tmp_nuxM;
struct blasfeo_dvec tmp_nbgM;
struct blasfeo_dvec tmp_nuxM;
struct blasfeo_dvec tmp_nbgM;

// struct blasfeo_dvec grad;
// struct blasfeo_dvec b2;
Expand All @@ -115,7 +115,7 @@ typedef struct {
struct blasfeo_dvec **ux; // pointer to ux in qp_out
struct blasfeo_dvec **pi; // pointer to pi in qp_out
struct blasfeo_dvec **lam; // pointer to lam in qp_out
int **idxb; // pointer to idxb in qp_in
int **idxb; // pointer to idxb in qp_in

} ocp_nlp_reg_convexify_memory;

Expand Down
Loading
Loading