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

Skip to content

GNRK: Gauss-Newton Runge-Kutta: IRK with nonlinear-least squares #698

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 31 commits into from
Aug 29, 2023
Merged

GNRK: Gauss-Newton Runge-Kutta: IRK with nonlinear-least squares #698

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
7a3a32a
GNRK squash commits:
FreyJo Oct 15, 2021
890b2c8
fix rebase
FreyJo Oct 21, 2022
37adbe8
IRK cost computation: think about z
FreyJo Nov 21, 2022
875b565
typehint
FreyJo Nov 21, 2022
895d965
fix rebase NLS cost
FreyJo Jan 9, 2023
bcfd4d8
relative diff betwenn sum(time_steps) and tf can be 1e-13
FreyJo May 10, 2023
dd43396
add setter for sim_method_newton_tol
FreyJo May 15, 2023
c4cb88e
Python x0 shape check
FreyJo Jul 26, 2023
1442414
Merge branch 'master' into GNRK
FreyJo Jul 27, 2023
59e2f3b
fix NLS cost evaluation in IRK
FreyJo Jul 27, 2023
56d2c97
add option to use EXPLICIT_RUNGE_KUTTA in implicit integrator
FreyJo Jul 27, 2023
458aacb
comments
FreyJo Jul 27, 2023
8c2ba85
Python check and add EXPLICIT_RUNGE_KUTTA
FreyJo Jul 27, 2023
cd47928
add test_cost_integration
FreyJo Jul 27, 2023
980c7c0
run test on actions
FreyJo Jul 27, 2023
731e875
fix NLS cost evaluation in IRK
FreyJo Jul 28, 2023
9035cb1
comments, cleanup
FreyJo Jul 28, 2023
d75fc27
found issue made test: examples/acados_python/tests/test_cost_integra…
FreyJo Jul 28, 2023
76bcdd4
fix the IRK NLS issue -> test_cost_integration passing
FreyJo Jul 28, 2023
5942702
fix memory leak
sandmaennchen Jul 30, 2023
18997d3
fix ny dim memory problem
sandmaennchen Jul 30, 2023
9719f32
fix transpose
FreyJo Jul 31, 2023
ea8a573
add test
FreyJo Jul 31, 2023
9d68064
add fail_on_nonzero_status to solve_for_x0
FreyJo Jul 31, 2023
64e88cc
add warning
FreyJo Aug 11, 2023
2f3f8c3
compare solver cost value with cost state value
sandmaennchen Aug 28, 2023
680cd98
go back to simple euler test and rename
sandmaennchen Aug 29, 2023
70da27d
compare solver cost and cost state
sandmaennchen Aug 29, 2023
e6fff5f
implement compute_cost correctly with cost integration
FreyJo Aug 29, 2023
90a6a5d
fix test names in CMakeLists
FreyJo Aug 29, 2023
ee5620d
Merge branch 'master' into GNRK
FreyJo Aug 29, 2023
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
55 changes: 47 additions & 8 deletions acados/ocp_nlp/ocp_nlp_common.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2083,6 +2083,27 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims,
int *nu = dims->nu;
int *ni = dims->ni;

/* prepare memory */
int cost_integration;
for (int i = 0; i < N; i++)
{
config->dynamics[i]->opts_get(config->dynamics[i], opts->dynamics[i],
"cost_computation", &cost_integration);
if (cost_integration)
{
// set pointers to cost function & gradient in integrator
double *cost_fun = config->cost[i]->memory_get_fun_ptr(mem->cost[i]);
struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(mem->cost[i]);
struct blasfeo_dmat *W_chol = config->cost[i]->memory_get_W_chol_ptr(mem->cost[i]);
struct blasfeo_dvec *y_ref = config->cost[i]->model_get_y_ref_ptr(in->cost[i]);

config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "cost_grad", cost_grad);
config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "cost_fun", cost_fun);
config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "W_chol", W_chol);
config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "y_ref", y_ref);
}
}

/* stage-wise multiple shooting lagrangian evaluation */

#if defined(ACADOS_WITH_OPENMP)
Expand Down Expand Up @@ -2428,20 +2449,20 @@ double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims,
#if defined(ACADOS_WITH_OPENMP)
#pragma omp parallel for
#endif
for (int i=0; i<=N; i++)
for (int i=0; i<N; i++)
{
// cost
config->cost[i]->compute_fun(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i],
mem->cost[i], work->cost[i]);
// dynamics: Note has to be first, because cost_integration might be used.
config->dynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i], in->dynamics[i],
opts->dynamics[i], mem->dynamics[i], work->dynamics[i]);
}
#if defined(ACADOS_WITH_OPENMP)
#pragma omp parallel for
#endif
for (int i=0; i<N; i++)
for (int i=0; i<=N; i++)
{
// dynamics
config->dynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i], in->dynamics[i],
opts->dynamics[i], mem->dynamics[i], work->dynamics[i]);
// cost
config->cost[i]->compute_fun(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i],
mem->cost[i], work->cost[i]);
}
#if defined(ACADOS_WITH_OPENMP)
#pragma omp parallel for
Expand Down Expand Up @@ -2693,6 +2714,8 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
}
}
}
if (opts->globalization != FIXED_STEP)
printf("alpha %f\n", alpha);

return alpha;
}
Expand Down Expand Up @@ -2977,8 +3000,24 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in
double* tmp_cost = NULL;
double total_cost = 0.0;

int cost_integration;

for (int i = 0; i <= N; i++)
{
if (i < N)
{
config->dynamics[i]->opts_get(config->dynamics[i], opts->dynamics[i], "cost_computation", &cost_integration);

if (cost_integration)
{
// evaluate at out, instead tmp_out;
config->dynamics[i]->memory_set_tmp_ux_ptr(out->ux+i, mem->dynamics[i]);
// evaluate: TODO, where to put cost?
config->dynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i],
in->dynamics[i], opts->dynamics[i], mem->dynamics[i], work->dynamics[i]);
}
}

// set pointers
// NOTE(oj): the cost compute function takes the tmp_ux_ptr as input,
// since it is also used for globalization,
Expand Down
2 changes: 2 additions & 0 deletions acados/ocp_nlp/ocp_nlp_cost_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@ typedef struct
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
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_);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
Expand Down
Loading