1 /**************************************************************************************************
2 * *
3 * This file is part of HPIPM. *
4 * *
5 * HPIPM -- High-Performance Interior Point Method. *
6 * Copyright (C) 2019 by Gianluca Frison. *
7 * Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8 * All rights reserved. *
9 * *
10 * The 2-Clause BSD License *
11 * *
12 * Redistribution and use in source and binary forms, with or without *
13 * modification, are permitted provided that the following conditions are met: *
14 * *
15 * 1. Redistributions of source code must retain the above copyright notice, this *
16 * list of conditions and the following disclaimer. *
17 * 2. Redistributions in binary form must reproduce the above copyright notice, *
18 * this list of conditions and the following disclaimer in the documentation *
19 * and/or other materials provided with the distribution. *
20 * *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND *
22 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
23 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
24 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR *
25 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
26 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
28 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
31 * *
32 * Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
33 * *
34 **************************************************************************************************/
35 // system
36 #include <stdlib.h>
37 #include <stdio.h>
38 #include <string.h>
39 // hpipm
40 #include "hpipm_d_ocp_qp_ipm.h"
41 // mex
42 #include "mex.h"
43
44
45
mexFunction(int nlhs,mxArray * plhs[],int nrhs,const mxArray * prhs[])46 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
47 {
48
49 // mexPrintf("\nin ocp_qp_sol_get\n");
50
51 long long *l_ptr;
52
53 int ii, jj;
54
55 /* RHS */
56
57 // ws
58 l_ptr = mxGetData( prhs[0] );
59 struct d_ocp_qp_ipm_ws *ws = (struct d_ocp_qp_ipm_ws *) *l_ptr;
60
61 // field
62 char *field = mxArrayToString( prhs[1] );
63
64 if(!strcmp(field, "status") | !strcmp(field, "iter"))
65 {
66 plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);
67 double *mat_ptr = mxGetPr( plhs[0] );
68 int tmp_int;
69 d_ocp_qp_ipm_get(field, ws, &tmp_int);
70 *mat_ptr = (double) tmp_int;
71 }
72 else if(!strcmp(field, "max_res_stat") | !strcmp(field, "max_res_eq") | !strcmp(field, "max_res_ineq") | !strcmp(field, "max_res_comp"))
73 {
74 plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);
75 double *mat_ptr = mxGetPr( plhs[0] );
76 d_ocp_qp_ipm_get(field, ws, mat_ptr);
77 }
78 else if(!strcmp(field, "stat"))
79 {
80 int iter;
81 int stat_m;
82 double *stat;
83 d_ocp_qp_ipm_get("iter", ws, &iter);
84 d_ocp_qp_ipm_get("stat_m", ws, &stat_m);
85 d_ocp_qp_ipm_get("stat", ws, &stat);
86 plhs[0] = mxCreateNumericMatrix(iter+1, stat_m+1, mxDOUBLE_CLASS, mxREAL);
87 double *mat_ptr = mxGetPr( plhs[0] );
88 for(ii=0; ii<iter+1; ii++)
89 {
90 mat_ptr[ii+0] = ii;
91 for(jj=0; jj<stat_m; jj++)
92 {
93 mat_ptr[ii+(jj+1)*(iter+1)] = stat[jj+ii*stat_m];
94 }
95 }
96 }
97 else
98 {
99 mexPrintf("\nocp_qp_solver_get: field not supported: %s\n", field);
100 return;
101 }
102
103 return;
104
105 }
106
107
108