Skip to content
Permalink
Browse files

Merge pull request #400 from FreyJo/GNSF_fully_linear

GNSF for fully linear systems
  • Loading branch information...
giaf committed Jul 10, 2019
2 parents 10ac374 + 4ae6df8 commit 48096c897d3eb42db71ce2d40e96950fe75c6289
Showing with 1,175 additions and 908 deletions.
  1. +0 −1 acados/dense_qp/dense_qp_hpipm.c
  2. +1 −1 acados/dense_qp/dense_qp_qpoases.c
  3. +10 −10 acados/ocp_nlp/ocp_nlp_constraints_bghp.c
  4. +2 −2 acados/ocp_nlp/ocp_nlp_cost_external.c
  5. +2 −2 acados/ocp_nlp/ocp_nlp_cost_ls.c
  6. +3 −3 acados/ocp_nlp/ocp_nlp_cost_nls.c
  7. +4 −0 acados/ocp_nlp/ocp_nlp_dynamics_cont.c
  8. +2 −2 acados/ocp_nlp/ocp_nlp_dynamics_disc.c
  9. +17 −21 acados/ocp_nlp/ocp_nlp_reg_convexify.c
  10. +3 −3 acados/ocp_nlp/ocp_nlp_reg_mirror.c
  11. +2 −3 acados/ocp_nlp/ocp_nlp_reg_project.c
  12. +3 −3 acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c
  13. +1 −1 acados/ocp_nlp/ocp_nlp_sqp.c
  14. +1 −1 acados/ocp_nlp/ocp_nlp_sqp_rti.c
  15. +2 −0 acados/sim/sim_common.c
  16. +2 −0 acados/sim/sim_common.h
  17. +958 −799 acados/sim/sim_gnsf.c
  18. +16 −7 acados/sim/sim_gnsf.h
  19. +13 −7 acados/sim/sim_irk_integrator.c
  20. +14 −6 acados/sim/sim_lifted_irk_integrator.c
  21. +2 −1 acados/utils/math.c
  22. +4 −6 examples/c/crane_nx9_model/crane_nx9_f_lo_fun_jac_x1k1uz.c
  23. +18 −3 examples/c/crane_nx9_model/crane_nx9_get_matrices_fun.c
  24. +6 −2 examples/c/crane_nx9_model/export_pure_linear_test_model.m
  25. +3 −0 examples/c/mass_spring_example.c
  26. +2 −5 examples/c/mass_spring_nmpc_example.c
  27. +7 −2 examples/c/sim_gnsf_crane.c
  28. +1 −6 examples/c/simple_dae_example.c
  29. +2 −1 experimental/interfaces/matlab/external_function_generation/sim/check_reformulation.m
  30. +7 −0 experimental/interfaces/matlab/external_function_generation/sim/detect_gnsf_structure.m
  31. +1 −0 ...imental/interfaces/matlab/external_function_generation/sim/determine_trivial_gnsf_transcription.m
  32. +14 −1 experimental/interfaces/matlab/external_function_generation/sim/generate_c_code_gnsf.m
  33. +42 −1 experimental/interfaces/matlab/external_function_generation/sim/reformulate_with_LOS.m
  34. +0 −2 interfaces/acados_c/ocp_nlp_interface.c
  35. +1 −0 test/sim/sim_test_dae.cpp
  36. +8 −6 test/sim/sim_test_hessian.cpp
  37. +1 −0 test/sim/sim_test_ode.cpp
@@ -225,7 +225,6 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void

// zero primal solution
// TODO add a check if warm start of first SQP iteration is implemented !!!!!!
int ii;
int nv = qp_in->dim->nv;
int ns = qp_in->dim->ns;
blasfeo_dvecse(nv+2*ns, 0.0, qp_out->v, 0);
@@ -120,7 +120,7 @@ void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts

void dense_qp_qpoases_opts_set(void *config_, void *opts_, const char *field, void *value)
{
dense_qp_qpoases_opts *opts = opts_;
// dense_qp_qpoases_opts *opts = opts_;

if (!strcmp(field, "tol_stat"))
{
@@ -263,19 +263,19 @@ static void ocp_nlp_constraints_bghp_get_nb(void *config_, void *dims_, int* val



static void ocp_nlp_constraints_bghp_get_nbx(void *config_, void *dims_, int* value)
{
ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_;
*value = dims->nbx;
}
// static void ocp_nlp_constraints_bghp_get_nbx(void *config_, void *dims_, int* value)
// {
// ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_;
// *value = dims->nbx;
// }



static void ocp_nlp_constraints_bghp_get_nbu(void *config_, void *dims_, int* value)
{
ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_;
*value = dims->nbu;
}
// static void ocp_nlp_constraints_bghp_get_nbu(void *config_, void *dims_, int* value)
// {
// ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_;
// *value = dims->nbu;
// }



@@ -300,8 +300,8 @@ void ocp_nlp_cost_external_opts_update(void *config_, void *dims_, void *opts_)

void ocp_nlp_cost_external_opts_set(void *config_, void *opts_, const char *field, void* value)
{
ocp_nlp_cost_config *config = config_;
ocp_nlp_cost_external_opts *opts = opts_;
// ocp_nlp_cost_config *config = config_;
// ocp_nlp_cost_external_opts *opts = opts_;

if(!strcmp(field, "exact_hess"))
{
@@ -430,8 +430,8 @@ void ocp_nlp_cost_ls_opts_update(void *config_, void *dims_, void *opts_)

void ocp_nlp_cost_ls_opts_set(void *config_, void *opts_, const char *field, void* value)
{
ocp_nlp_cost_config *config = config_;
ocp_nlp_cost_ls_opts *opts = opts_;
// ocp_nlp_cost_config *config = config_;
// ocp_nlp_cost_ls_opts *opts = opts_;

if(!strcmp(field, "exact_hess"))
{
@@ -244,8 +244,8 @@ int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_,
ocp_nlp_cost_nls_dims *dims = dims_;
ocp_nlp_cost_nls_model *model = model_;

int nx = dims->nx;
int nu = dims->nu;
// int nx = dims->nx;
// int nu = dims->nu;
int ny = dims->ny;
int ns = dims->ns;

@@ -372,7 +372,7 @@ void ocp_nlp_cost_nls_opts_update(void *config_, void *dims_, void *opts_)

void ocp_nlp_cost_nls_opts_set(void *config_, void *opts_, const char *field, void* value)
{
ocp_nlp_cost_config *config = config_;
// ocp_nlp_cost_config *config = config_;
ocp_nlp_cost_nls_opts *opts = opts_;

if(!strcmp(field, "gauss_newton_hess"))
@@ -640,10 +640,14 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void *

// initialize seeds
// TODO fix dims if nx!=nx1 !!!!!!!!!!!!!!!!!
// set S_forw = [eye(nx), zeros(nx x nu)]
for(jj = 0; jj < nx1 * (nx + nu); jj++)
work->sim_in->S_forw[jj] = 0.0;
for(jj = 0; jj < nx1; jj++)
work->sim_in->S_forw[jj * (nx + 1)] = 1.0;
work->sim_in->identity_seed = true;

// adjoint seed
for(jj = 0; jj < nx + nu; jj++)
work->sim_in->S_adj[jj] = 0.0;
blasfeo_unpack_dvec(nx1, mem->pi, 0, work->sim_in->S_adj);
@@ -367,7 +367,7 @@ int ocp_nlp_dynamics_disc_workspace_calculate_size(void *config_, void *dims_, v

int nx = dims->nx;
int nu = dims->nu;
int nx1 = dims->nx1;
// int nx1 = dims->nx1;

int size = 0;

@@ -396,7 +396,7 @@ static void ocp_nlp_dynamics_disc_cast_workspace(void *config_, void *dims_, voi

int nx = dims->nx;
int nu = dims->nu;
int nx1 = dims->nx1;
// int nx1 = dims->nx1;

char *c_ptr = (char *) work_;
c_ptr += sizeof(ocp_nlp_dynamics_disc_workspace);
@@ -264,7 +264,8 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims,
for (ii=0; ii<=N; ii++)
assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], &c_ptr);

assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->tmp_RSQ, &c_ptr);

assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->tmp_RSQ, &c_ptr);

assign_and_advance_blasfeo_dvec_mem(nuxM, &mem->tmp_nuxM, &c_ptr);
assign_and_advance_blasfeo_dvec_mem(nbgM, &mem->tmp_nbgM, &c_ptr);
@@ -286,8 +287,6 @@ void ocp_nlp_reg_convexify_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct b
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;

for(ii=0; ii<=N; ii++)
{
@@ -307,8 +306,8 @@ void ocp_nlp_reg_convexify_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blas
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<=N; ii++)
{
@@ -328,8 +327,8 @@ void ocp_nlp_reg_convexify_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct bl
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<N; ii++)
{
@@ -349,8 +348,8 @@ void ocp_nlp_reg_convexify_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasf
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<N; ii++)
{
@@ -388,9 +387,6 @@ void ocp_nlp_reg_convexify_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct bla
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
int *ng = dims->ng;

for(ii=0; ii<=N; ii++)
{
@@ -410,8 +406,8 @@ void ocp_nlp_reg_convexify_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blas
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<=N; ii++)
{
@@ -431,8 +427,8 @@ void ocp_nlp_reg_convexify_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blas
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<N; ii++)
{
@@ -452,9 +448,9 @@ void ocp_nlp_reg_convexify_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct bla
int ii;

int N = dims->N;
int *nbu = dims->nbu;
int *nbx = dims->nbx;
int *ng = dims->ng;
// int *nbu = dims->nbu;
// int *nbx = dims->nbx;
// int *ng = dims->ng;

for(ii=0; ii<=N; ii++)
{
@@ -679,9 +675,9 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di
void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
{
ocp_nlp_reg_convexify_memory *mem = mem_;
ocp_nlp_reg_convexify_opts *opts = opts_;
// ocp_nlp_reg_convexify_opts *opts = opts_;

int ii, ss;
int ii;

int N = dims->N;
int *nx = dims->nx;
@@ -159,8 +159,8 @@ void ocp_nlp_reg_mirror_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blas
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<=N; ii++)
{
@@ -261,7 +261,7 @@ void ocp_nlp_reg_mirror_regularize_hessian(void *config, ocp_nlp_reg_dims *dims,

int *nx = dims->nx;
int *nu = dims->nu;
int N = dims->N;
// int N = dims->N;

for(ii=0; ii<=dims->N; ii++)
{
@@ -159,8 +159,8 @@ void ocp_nlp_reg_project_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct bla
int ii;

int N = dims->N;
int *nx = dims->nx;
int *nu = dims->nu;
// int *nx = dims->nx;
// int *nu = dims->nu;

for(ii=0; ii<=N; ii++)
{
@@ -261,7 +261,6 @@ void ocp_nlp_reg_project_regularize_hessian(void *config, ocp_nlp_reg_dims *dims

int *nx = dims->nx;
int *nu = dims->nu;
int N = dims->N;

for(ii=0; ii<=dims->N; ii++)
{
@@ -405,7 +405,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg
for(kk=jj+1; kk<nu[ss]; kk++)
{
tmp_el = BLASFEO_DMATEL(L3, kk, kk);
if(tmp_el<pivot & tmp_el>-pivot)
if((tmp_el<pivot) & (tmp_el>-pivot))
{
pivot = BLASFEO_DMATEL(L3, kk, kk);
idx = kk;
@@ -423,7 +423,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg
}
// bottom rectangle
for(kk=idx+1; kk<nu[ss]+nx[ss]; kk++)
{
{
tmp_el = BLASFEO_DMATEL(L3, kk, jj);
BLASFEO_DMATEL(L3, kk, jj) = BLASFEO_DMATEL(L3, kk, idx);
BLASFEO_DMATEL(L3, kk, idx) = tmp_el;
@@ -432,7 +432,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg
}

pivot = BLASFEO_DMATEL(L3, jj, jj);
if(pivot<opts->min_pivot & pivot>-opts->min_pivot)
if ((pivot<opts->min_pivot) & (pivot>-opts->min_pivot))
{
if(pivot<0.0)
pivot = opts->min_pivot;
@@ -1377,7 +1377,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
// exit(1);


if (qp_status!=ACADOS_SUCCESS & qp_status!=ACADOS_MAXITER)
if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
{
// print_ocp_qp_in(work->qp_in);

@@ -1262,7 +1262,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
mem->stat[mem->stat_n*1+0] = qp_status;
mem->stat[mem->stat_n*1+1] = qp_iter;

if (qp_status!=ACADOS_SUCCESS & qp_status!=ACADOS_MAXITER)
if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
{
// print_ocp_qp_in(work->qp_in);

@@ -121,6 +121,8 @@ sim_in *sim_in_assign(void *config_, void *dims, void *raw_memory)
assign_and_advance_double(nx * NF, &in->S_forw, &c_ptr);
assign_and_advance_double(NF, &in->S_adj, &c_ptr);

in->identity_seed = false;

in->model = config->model_assign(config, dims, c_ptr);
c_ptr += config->model_calculate_size(config, dims);

@@ -66,6 +66,8 @@ typedef struct
double *S_forw; // forward seed [Sx, Su]
double *S_adj; // backward seed // TODO rename seed_adj ???

bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)]

void *model;

double T; // simulation time

0 comments on commit 48096c8

Please sign in to comment.
You can’t perform that action at this time.