From aef79be06b81881e3bc37f2b9ff503726a7efafd Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 5 Jul 2024 17:19:03 +0200 Subject: [PATCH 1/3] implement getter for pcond_Q, pcond_R, pcond_S --- acados/ocp_qp/ocp_qp_xcond_solver.c | 33 +++++++++++++++++-- interfaces/acados_c/ocp_nlp_interface.c | 33 +++++++++++++++++++ .../acados_template/acados_ocp_solver.py | 10 ++++-- 3 files changed, 71 insertions(+), 5 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.c b/acados/ocp_qp/ocp_qp_xcond_solver.c index 95fbee0865..1a127e1308 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -143,10 +143,37 @@ void ocp_qp_xcond_solver_dims_set_(void *config_, ocp_qp_xcond_solver_dims *dims void ocp_qp_xcond_solver_dims_get_(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value) { - // get from orig_dims - ocp_qp_dims_get(config_, dims->orig_dims, stage, field, value); + // extract module name + char module[MAX_STR_LEN]; + char *ptr_module = NULL; + int module_length; + char *char_ = strchr(field, '_'); + if (char_!=NULL) + { + module_length = char_-field; + for (int ii=0; iixcond; + void *xcond_qp_dims; + xcond->dims_get(xcond, dims->xcond_dims, "xcond_dims", &xcond_qp_dims); + ocp_qp_dims_get(config_, xcond_qp_dims, stage, &field[6], value); + return; + } + else + { + // get from orig_dims + ocp_qp_dims_get(config_, dims->orig_dims, stage, field, value); + + return; + } } diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 5b5b3f92d0..3ace9582c9 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -855,6 +855,21 @@ void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, o config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "nbu", &dims_out[0]); dims_out[1] = 1; } + else if (!strcmp(field, "pcond_R")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[1]); + } + else if (!strcmp(field, "pcond_Q")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); + } + else if (!strcmp(field, "pcond_S")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); + } else { printf("\nerror: ocp_nlp_qp_dims_get_from_attr: field %s not available\n", field); @@ -1265,6 +1280,24 @@ void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_so } xcond_solver_config->solver_get(xcond_solver_config, nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, field, stage, value, size1, size2); } + else if (!strcmp(field, "pcond_Q")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(config, solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_Q(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_R")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(config, solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_R(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_S")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(config, solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_S(stage, pcond_qp_in, value); + } else { printf("\nerror: ocp_nlp_get_at_stage: field %s not available\n", field); diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 39f5a5551d..32248c5ada 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -263,6 +263,7 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file= self.__qp_cost_fields = ['Q', 'R', 'S', 'q', 'r'] self.__qp_constraint_fields = ['C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] self.__qp_pc_hpipm_fields = ['P', 'K', 'Lr', 'p'] + self.__qp_pc_fields = ['pcond_Q', 'pcond_R', 'pcond_S'] # set arg and res types self.__acados_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] @@ -1374,7 +1375,10 @@ def get_from_qp_in(self, stage_: int, field_: str): :param stage: integer corresponding to shooting node :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] - Note: additional supported fields are ['P', 'K', 'Lr'], which can be extracted form QP solver PARTIAL_CONDENSING_HPIPM. + Note: + - additional supported fields are ['P', 'K', 'Lr'], which can be extracted form QP solver PARTIAL_CONDENSING_HPIPM. + - for PARTIAL_CONDENSING_* QP solvers, the following additional fields are available: + ['pcond_Q', 'pcond_R', 'pcond_S'] """ # idx* should be added too.. if not isinstance(stage_, int): @@ -1383,13 +1387,15 @@ def get_from_qp_in(self, stage_: int, field_: str): raise Exception("stage should be <= self.N") if field_ in self.__qp_dynamics_fields and stage_ >= self.N: raise ValueError(f"dynamics field {field_} not available at terminal stage") - if field_ not in self.__qp_dynamics_fields + self.__qp_cost_fields + self.__qp_constraint_fields + self.__qp_pc_hpipm_fields: + if field_ not in self.__qp_dynamics_fields + self.__qp_cost_fields + self.__qp_constraint_fields + self.__qp_pc_hpipm_fields + self.__qp_pc_fields: raise Exception(f"field {field_} not supported.") if field_ in self.__qp_pc_hpipm_fields: if self.acados_ocp.solver_options.qp_solver != "PARTIAL_CONDENSING_HPIPM" or self.acados_ocp.solver_options.qp_solver_cond_N != self.acados_ocp.dims.N: raise Exception(f"field {field_} only works for PARTIAL_CONDENSING_HPIPM QP solver with qp_solver_cond_N == N.") if field_ in ["P", "K", "p"] and stage_ == 0 and self.acados_ocp.dims.nbxe_0 > 0: raise Exception(f"getting field {field_} at stage 0 only works without x0 elimination (see nbxe_0).") + if field_ in self.__qp_pc_fields and not self.acados_ocp.solver_options.qp_solver.startswith("PARTIAL_CONDENSING"): + raise Exception(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") field = field_.encode('utf-8') stage = c_int(stage_) From ad6064acc6cc2614417e70f48a08ea91702f4263 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 5 Jul 2024 14:19:18 +0200 Subject: [PATCH 2/3] add dimension names in C interface get_from_attr functions --- interfaces/acados_c/ocp_nlp_interface.c | 28 ++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 3ace9582c9..0a043f631a 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -612,7 +612,7 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n int dims_value = -1; // ocp_nlp_dims - if (!strcmp(field, "x")) + if (!strcmp(field, "x") || !strcmp(field, "nx")) { return dims->nx[stage]; } @@ -620,15 +620,15 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n { return dims->nx[stage+1]; } - else if (!strcmp(field, "u")) + else if (!strcmp(field, "u") || !strcmp(field, "nu")) { return dims->nu[stage]; } - else if (!strcmp(field, "z")) + else if (!strcmp(field, "z") || !strcmp(field, "nz")) { return dims->nz[stage]; } - else if (!strcmp(field, "p")) + else if (!strcmp(field, "p") || !strcmp(field, "np")) { return dims->np[stage]; } @@ -662,32 +662,32 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n return dims_value; } // ocp_nlp_constraints_dims - else if (!strcmp(field, "lbx") || !strcmp(field, "ubx")) + else if (!strcmp(field, "lbx") || !strcmp(field, "ubx") || !strcmp(field, "nbx")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nbx", &dims_value); return dims_value; } - else if (!strcmp(field, "lbu") || !strcmp(field, "ubu")) + else if (!strcmp(field, "lbu") || !strcmp(field, "ubu") || !strcmp(field, "nbu")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nbu", &dims_value); return dims_value; } - else if (!strcmp(field, "lg") || !strcmp(field, "ug")) + else if (!strcmp(field, "lg") || !strcmp(field, "ug") || !strcmp(field, "ng")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "ng", &dims_value); return dims_value; } - else if (!strcmp(field, "lh") || !strcmp(field, "uh")) + else if (!strcmp(field, "lh") || !strcmp(field, "uh") || !strcmp(field, "nh")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nh", &dims_value); return dims_value; } // ocp_nlp_cost_dims - else if (!strcmp(field, "y_ref") || !strcmp(field, "yref")) + else if (!strcmp(field, "y_ref") || !strcmp(field, "yref") || !strcmp(field, "ny")) { config->cost[stage]->dims_get(config->cost[stage], dims->cost[stage], "ny", &dims_value); @@ -707,31 +707,31 @@ void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims // vectors first dims_out[1] = 0; // ocp_nlp_constraints_dims - if (!strcmp(field, "lbx") || !strcmp(field, "ubx")) + if (!strcmp(field, "lbx") || !strcmp(field, "ubx") || !strcmp(field, "nbx")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nbx", &dims_out[0]); return; } - else if (!strcmp(field, "uphi")) + else if (!strcmp(field, "uphi") || !strcmp(field, "nphi")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nphi", &dims_out[0]); return; } - else if (!strcmp(field, "lbu") || !strcmp(field, "ubu")) + else if (!strcmp(field, "lbu") || !strcmp(field, "ubu") || !strcmp(field, "nbu")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nbu", &dims_out[0]); return; } - else if (!strcmp(field, "lg") || !strcmp(field, "ug")) + else if (!strcmp(field, "lg") || !strcmp(field, "ug") || !strcmp(field, "ng")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "ng", &dims_out[0]); return; } - else if (!strcmp(field, "lh") || !strcmp(field, "uh")) + else if (!strcmp(field, "lh") || !strcmp(field, "uh") || !strcmp(field, "nh")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], "nh", &dims_out[0]); From 50ca1a75d31f5d9a9a878cbab9d01a8f21781a27 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 5 Jul 2024 17:31:12 +0200 Subject: [PATCH 3/3] test getting pcond matrices and sanity check their dimensions --- .../ocp/nonuniform_discretization_example.py | 36 +++++++++++++++++-- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py b/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py index 575acd2c0f..8323f6798b 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py +++ b/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py @@ -38,6 +38,10 @@ import scipy.linalg from utils import plot_pendulum +import matplotlib.pyplot as plt + +import casadi as ca + TOL = 1e-7 def main(discretization='shooting_nodes'): @@ -147,9 +151,6 @@ def main(discretization='shooting_nodes'): simulink_opts = json.load(f) ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', simulink_opts = simulink_opts, verbose=False) - # ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - - simX = np.zeros((N+1, nx)) simU = np.zeros((N, nu)) @@ -178,6 +179,35 @@ def main(discretization='shooting_nodes'): simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") + # get condensed Hessian + pcond_H = [] + pcond_Q = [] + pcond_R = [] + pcond_S = [] + for i in range(ocp.solver_options.qp_solver_cond_N+1): + pcond_Q.append(ocp_solver.get_from_qp_in(i, "pcond_Q")) + pcond_R.append(ocp_solver.get_from_qp_in(i, "pcond_R")) + pcond_S.append(ocp_solver.get_from_qp_in(i, "pcond_S")) + + pcond_RSQ = ca.blockcat(pcond_Q[-1], pcond_S[-1].T, pcond_S[-1], pcond_R[-1]).full() + # copy lower triangular part to upper triangular part + pcond_RSQ = np.tril(pcond_RSQ) + np.tril(pcond_RSQ, -1).T + pcond_H.append(pcond_RSQ) + + # pcond_H = ocp_solver.get_from_qp_in(ocp.solver_options.qp_solver_cond_N, "pcond_H") + # print("pcond_H", pcond_H) + # pcond_H_mat = scipy.linalg.block_diag(*pcond_H) + # plt.spy(pcond_H_mat) + # plt.show() + + # check dimensions of partially condensed matrices + assert pcond_Q[0].shape == (0, 0) + for i in range(1, ocp.solver_options.qp_solver_cond_N+1): + assert pcond_Q[i].shape == (nx, nx) + for i in range(ocp.solver_options.qp_solver_cond_N+1): + block_size = ocp.solver_options.qp_solver_cond_block_size[i] + assert pcond_R[i].shape == (nu*block_size, nu*block_size) + print("inequality multipliers at stage 1") print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1 print("slack values at stage 1")