ocp_qp: add ws getter for p (only valid for the unconstr case)

examples/c/example_d_ocp_qp_unconstr.c

@@ -147,12 +147,12 @@ int main()
 
 #if 0
 	// keep x0 as an optimization variable, set its value using equal upper-lower bounds
-	for(ii=1; ii<=N; ii++)
-		nbx[ii] = 0;
+//	for(ii=1; ii<=N; ii++)
+//		nbx[ii] = 0;
 
-	hlb[0] = x0;
-	hub[0] = x0;
-	hidxbx[0] = idxbx0;
+//	hlbx[0] = x0;
+//	hubx[0] = x0;
+//	hidxbx[0] = idxbx0;
 #else
 	// remove x0 from the optimization variables
 	nx[0] = 0;
@@ -233,7 +233,7 @@ int main()
 	d_ocp_qp_ipm_arg_set_tol_comp(&tol_comp, &arg);
 	d_ocp_qp_ipm_arg_set_reg_prim(&reg_prim, &arg);
 	d_ocp_qp_ipm_arg_set_warm_start(&warm_start, &arg);
-//	d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg);
+	d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg);
 
 /************************************************
 * ipm workspace
@@ -367,6 +367,70 @@ int main()
 
 	printf("\nocp ipm time = %e [s]\n\n", time_ipm);
 
+/************************************************
+* get riccati matrices and vectors
+************************************************/
+
+#if 1
+	printf("\nget Riccati recursion matrices and vectors\n");
+
+	double *Lr0 = malloc(nu[0]*nu[0]*sizeof(double));
+	double *Ls0 = malloc(nx[0]*nu[0]*sizeof(double));
+	double *P0 = malloc(nx[0]*nx[0]*sizeof(double));
+	double *lr0 = malloc(nu[0]*sizeof(double));
+	double *p0 = malloc(nx[0]*sizeof(double));
+
+	double *Lr1 = malloc(nu[1]*nu[1]*sizeof(double));
+	double *Ls1 = malloc(nx[1]*nu[1]*sizeof(double));
+	double *P1 = malloc(nx[1]*nx[1]*sizeof(double));
+	double *lr1 = malloc(nu[1]*sizeof(double));
+	double *p1 = malloc(nx[1]*sizeof(double));
+
+	d_ocp_qp_ipm_get_ric_Lr(0, &workspace, Lr0);
+	printf("\nLr0\n");
+	d_print_exp_mat(nu[0], nu[0], Lr0, nu[0]);
+	d_ocp_qp_ipm_get_ric_Ls(0, &workspace, Ls0);
+	printf("\nLs0\n");
+	d_print_exp_mat(nx[0], nu[0], Ls0, nx[0]);
+	d_ocp_qp_ipm_get_ric_P(0, &workspace, P0);
+	printf("\nP0\n");
+	d_print_exp_mat(nx[0], nx[0], P0, nx[0]);
+	d_ocp_qp_ipm_get_ric_lr(0, &workspace, lr0);
+	printf("\nlr0 (valid only in the unconstrained case)\n");
+	d_print_exp_mat(1, nu[0], lr0, 1);
+	d_ocp_qp_ipm_get_ric_p(0, &workspace, p0);
+	printf("\np0 (valid only in the unconstrained case)\n");
+	d_print_exp_mat(1, nx[0], p0, 1);
+
+	d_ocp_qp_ipm_get_ric_Lr(1, &workspace, Lr1);
+	printf("\nLr1\n");
+	d_print_exp_mat(nu[1], nu[1], Lr1, nu[1]);
+	d_ocp_qp_ipm_get_ric_Ls(1, &workspace, Ls1);
+	printf("\nLs1\n");
+	d_print_exp_mat(nx[1], nu[1], Ls1, nx[1]);
+	d_ocp_qp_ipm_get_ric_P(1, &workspace, P1);
+	printf("\nP1\n");
+	d_print_exp_mat(nx[1], nx[1], P1, nx[1]);
+	d_ocp_qp_ipm_get_ric_lr(1, &workspace, lr1);
+	printf("\nlr1 (valid only in the unconstrained case)\n");
+	d_print_exp_mat(1, nu[1], lr1, 1);
+	d_ocp_qp_ipm_get_ric_p(1, &workspace, p1);
+	printf("\np1 (valid only in the unconstrained case)\n");
+	d_print_exp_mat(1, nx[1], p1, 1);
+
+	free(Lr0);
+	free(Ls0);
+	free(P0);
+	free(lr0);
+	free(p0);
+
+	free(Lr1);
+	free(Ls1);
+	free(P1);
+	free(lr1);
+	free(p1);
+#endif
+
 /************************************************
 * free memory and return
 ************************************************/

include/hpipm_d_ocp_qp_ipm.h

@@ -191,8 +191,10 @@ void d_ocp_qp_ipm_get_ric_Lr(int stage, struct d_ocp_qp_ipm_ws *ws, double *Lr);
 void d_ocp_qp_ipm_get_ric_Ls(int stage, struct d_ocp_qp_ipm_ws *ws, double *Ls);
 //
 void d_ocp_qp_ipm_get_ric_P(int stage, struct d_ocp_qp_ipm_ws *ws, double *P);
-//
+// valid only in the unconstrained case
 void d_ocp_qp_ipm_get_ric_lr(int stage, struct d_ocp_qp_ipm_ws *ws, double *lr);
+// valid only in the unconstrained case
+void d_ocp_qp_ipm_get_ric_p(int stage, struct d_ocp_qp_ipm_ws *ws, double *p);
 //
 void d_ocp_qp_ipm_solve(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
 //

include/hpipm_s_ocp_qp_ipm.h

@@ -193,8 +193,10 @@ void s_ocp_qp_ipm_get_ric_Lr(int stage, struct s_ocp_qp_ipm_ws *ws, float *Lr);
 void s_ocp_qp_ipm_get_ric_Ls(int stage, struct s_ocp_qp_ipm_ws *ws, float *Ls);
 //
 void s_ocp_qp_ipm_get_ric_P(int stage, struct s_ocp_qp_ipm_ws *ws, float *P);
-//
+// valid only in the unconstrained case
 void s_ocp_qp_ipm_get_ric_lr(int stage, struct s_ocp_qp_ipm_ws *ws, float *lr);
+// valid only in the unconstrained case
+void s_ocp_qp_ipm_get_ric_p(int stage, struct s_ocp_qp_ipm_ws *ws, float *p);
 //
 void s_ocp_qp_ipm_solve(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
 //

ocp_qp/d_ocp_qp_ipm.c

@@ -99,13 +99,16 @@
 #define PRINT_STRVEC blasfeo_print_dvec
 #define PRINT_TRAN_STRVEC blasfeo_print_tran_dvec
 #define REAL double
+#define ROWEX blasfeo_drowex
 #define SIZE_STRMAT blasfeo_memsize_dmat
 #define SIZE_STRVEC blasfeo_memsize_dvec
 #define SOLVE_KKT_STEP_OCP_QP d_solve_kkt_step_ocp_qp
 #define STRMAT blasfeo_dmat
 #define STRVEC blasfeo_dvec
 #define SYRK_LN blasfeo_dsyrk_ln
 #define TRCP_L blasfeo_dtrcp_l
+#define TRMV_LNN blasfeo_dtrmv_lnn
+#define TRMV_LTN blasfeo_dtrmv_ltn
 #define TRTR_L blasfeo_dtrtr_l
 #define UNPACK_MAT blasfeo_unpack_dmat
 #define UNPACK_VEC blasfeo_unpack_dvec
@@ -152,6 +155,7 @@
 #define OCP_QP_IPM_GET_RIC_LS d_ocp_qp_ipm_get_ric_Ls
 #define OCP_QP_IPM_GET_RIC_P d_ocp_qp_ipm_get_ric_P
 #define OCP_QP_IPM_GET_RIC_LR_VEC d_ocp_qp_ipm_get_ric_lr
+#define OCP_QP_IPM_GET_RIC_P_VEC d_ocp_qp_ipm_get_ric_p
 #define OCP_QP_IPM_SOLVE d_ocp_qp_ipm_solve
 #define OCP_QP_IPM_PREDICT d_ocp_qp_ipm_predict
 #define OCP_QP_IPM_SENS d_ocp_qp_ipm_sens

ocp_qp/s_ocp_qp_ipm.c

@@ -99,13 +99,16 @@
 #define PRINT_STRVEC blasfeo_print_svec
 #define PRINT_TRAN_STRVEC blasfeo_print_tran_svec
 #define REAL float
+#define ROWEX blasfeo_srowex
 #define SIZE_STRMAT blasfeo_memsize_smat
 #define SIZE_STRVEC blasfeo_memsize_svec
 #define SOLVE_KKT_STEP_OCP_QP s_solve_kkt_step_ocp_qp
 #define STRMAT blasfeo_smat
 #define STRVEC blasfeo_svec
 #define SYRK_LN blasfeo_ssyrk_ln
 #define TRCP_L blasfeo_strcp_l
+#define TRMV_LNN blasfeo_strmv_lnn
+#define TRMV_LTN blasfeo_strmv_ltn
 #define TRTR_L blasfeo_strtr_l
 #define UNPACK_MAT blasfeo_unpack_smat
 #define UNPACK_VEC blasfeo_unpack_svec
@@ -151,6 +154,7 @@
 #define OCP_QP_IPM_GET_RIC_LS s_ocp_qp_ipm_get_ric_Ls
 #define OCP_QP_IPM_GET_RIC_P s_ocp_qp_ipm_get_ric_P
 #define OCP_QP_IPM_GET_RIC_LR_VEC s_ocp_qp_ipm_get_ric_lr
+#define OCP_QP_IPM_GET_RIC_P_VEC s_ocp_qp_ipm_get_ric_p
 #define OCP_QP_IPM_SOLVE s_ocp_qp_ipm_solve
 #define OCP_QP_IPM_PREDICT s_ocp_qp_ipm_predict
 #define OCP_QP_IPM_SENS s_ocp_qp_ipm_sens

ocp_qp/x_ocp_qp_ipm.c

@@ -1101,6 +1101,7 @@ void OCP_QP_IPM_GET_RIC_P(int stage, struct OCP_QP_IPM_WS *ws, REAL *P)
 
 
 
+// XXX valid only in the unconstrained case !!!
 void OCP_QP_IPM_GET_RIC_LR_VEC(int stage, struct OCP_QP_IPM_WS *ws, REAL *lr)
 	{
 	int *nu = ws->dim->nu;
@@ -1114,6 +1115,29 @@ void OCP_QP_IPM_GET_RIC_LR_VEC(int stage, struct OCP_QP_IPM_WS *ws, REAL *lr)
 
 
 
+// XXX valid only in the unconstrained case !!!
+void OCP_QP_IPM_GET_RIC_P_VEC(int stage, struct OCP_QP_IPM_WS *ws, REAL *p)
+	{
+	int *nu = ws->dim->nu;
+	int *nx = ws->dim->nx;
+
+	int nu0 = nu[stage];
+	int nx0 = nx[stage];
+
+	if(ws->square_root_alg)
+		{
+		ROWEX(nx0, 1.0, ws->L+stage, nu0+nx0, nu0, ws->tmp_nxM, 0);
+		TRMV_LNN(nx0, nx0, ws->L+stage, nu0, nu0, ws->tmp_nxM, 0, ws->tmp_nxM, 0);
+		UNPACK_VEC(nx0, ws->tmp_nxM, 0, p);
+		}
+	else
+		{
+		UNPACK_MAT(1, nx0, ws->P+stage, nx0, 0, p, 1);
+		}
+	}
+
+
+
 void OCP_QP_IPM_SOLVE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WS *ws)
 	{
 

test_problems/test_d_ocp.c

@@ -873,7 +873,7 @@ int main()
 	double reg_prim = 1e-12;
 	int warm_start = 0;
 	int pred_corr = 1;
-	int ric_alg = 1;
+	int ric_alg = 0;
 
 	d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg);
 	d_ocp_qp_ipm_arg_set_iter_max(&iter_max, &arg);
@@ -1090,40 +1090,6 @@ int main()
 
 	printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
 
-/************************************************
-* get riccati matrices
-************************************************/
-
-#if 0
-	double *Lr = malloc(nu_*nu_*sizeof(double));
-	double *Ls = malloc(nx_*nu_*sizeof(double));
-	double *P = malloc(nx_*nx_*sizeof(double));
-	double *lr = malloc(nu_*sizeof(double));
-
-	d_ocp_qp_ipm_get_ric_Lr(0, &workspace, Lr);
-	d_print_exp_mat(nu_, nu_, Lr, nu_);
-	d_ocp_qp_ipm_get_ric_Ls(0, &workspace, Ls);
-	d_print_exp_mat(nx_, nu_, Ls, nx_);
-	d_ocp_qp_ipm_get_ric_P(0, &workspace, P);
-	d_print_exp_mat(nx_, nx_, P, nx_);
-	d_ocp_qp_ipm_get_ric_lr(0, &workspace, lr);
-	d_print_exp_mat(1, nu_, lr, 1);
-
-	d_ocp_qp_ipm_get_ric_Lr(1, &workspace, Lr);
-	d_print_exp_mat(nu_, nu_, Lr, nu_);
-	d_ocp_qp_ipm_get_ric_Ls(1, &workspace, Ls);
-	d_print_exp_mat(nx_, nu_, Ls, nx_);
-	d_ocp_qp_ipm_get_ric_P(1, &workspace, P);
-	d_print_exp_mat(nx_, nx_, P, nx_);
-	d_ocp_qp_ipm_get_ric_lr(1, &workspace, lr);
-	d_print_exp_mat(1, nu_, lr, 1);
-
-	free(Lr);
-	free(Ls);
-	free(P);
-	free(lr);
-#endif
-
 /************************************************
 * free memory
 ************************************************/