1 /*
2 * This file is part of qpOASES.
3 *
4 * qpOASES -- An Implementation of the Online Active Set Strategy.
5 * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka,
6 * Christian Kirches et al. All rights reserved.
7 *
8 * qpOASES is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU Lesser General Public
10 * License as published by the Free Software Foundation; either
11 * version 2.1 of the License, or (at your option) any later version.
12 *
13 * qpOASES is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16 * See the GNU Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public
19 * License along with qpOASES; if not, write to the Free Software
20 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21 *
22 */
23
24
25 /**
26 * \file interfaces/scilab/qpOASESroutines.cpp
27 * \author Holger Diedam, Hans Joachim Ferreau
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Interface that enables to call qpOASES from scilab
32 * (C++ file to provide an interface between the files that
33 * have to be compiled with gcc and the qpOASES library).
34 *
35 */
36
37
38 #include <scilab/Scierror.h>
39
40 #include <qpOASES.hpp>
41
42
43 USING_NAMESPACE_QPOASES
44
45 /*extern "C" {
46 #include "../c/qpOASES_wrapper.h"
47 }*/
48
49
50 /* global pointers to qpOASES objects */
51 static QProblem* qp = 0;
52 static QProblemB* qpb = 0;
53 static SQProblem* sqp = 0;
54
55
56 extern "C"
57 {
58 void sci_qpOASES( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
59 int_t *nV, int_t* nC, int_t* nWSR,
60 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
61 );
62
63 void sci_QProblem_init( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
64 int_t* nV, int_t* nC, int_t* nWSR,
65 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
66 );
67 void sci_QProblemB_init( real_t* H, real_t* g, real_t* lb, real_t* ub,
68 int_t* nV, int_t* nWSR,
69 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
70 );
71 void sci_SQProblem_init( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
72 int_t* nV, int_t* nC, int_t* nWSR,
73 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
74 );
75
76 void sci_QProblem_hotstart( real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
77 int_t* nWSR,
78 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
79 );
80 void sci_QProblemB_hotstart( real_t* g, real_t* lb, real_t* ub,
81 int_t* nWSR,
82 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
83 );
84 void sci_SQProblem_hotstart( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
85 int_t* nWSR,
86 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
87 );
88
89 void sci_QProblem_cleanup( );
90 void sci_QProblemB_cleanup( );
91 void sci_SQProblem_cleanup( );
92 } /* extern "C" */
93
94
95
96 /*
97 * t r a n s f o r m A
98 */
transformA(real_t * A,int_t nV,int_t nC)99 void transformA( real_t* A, int_t nV, int_t nC )
100 {
101 int_t i, j;
102
103 real_t* A_tmp = new real_t[nC*nV];
104
105 for( i=0; i<nV*nC; ++i )
106 A_tmp[i] = A[i];
107
108 for( i=0; i<nC; ++i )
109 for( j=0; j<nV; ++j )
110 A[i*nV + j] = A_tmp[j*nC + i];
111
112 delete[] A_tmp;
113
114 return;
115 }
116
117
118 /*
119 * q p O A S E S
120 */
sci_qpOASES(real_t * H,real_t * g,real_t * A,real_t * lb,real_t * ub,real_t * lbA,real_t * ubA,int_t * nV,int_t * nC,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)121 void sci_qpOASES( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
122 int_t *nV, int_t* nC, int_t* nWSR,
123 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
124 )
125 {
126 /* transform A into C style matrix */
127 transformA( A, *nV,*nC );
128
129 /* setup and solve initial QP */
130 QProblem single_qp( *nV,*nC );
131 single_qp.setPrintLevel( PL_LOW );
132 returnValue returnvalue = single_qp.init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
133
134 /* assign lhs arguments */
135 single_qp.getPrimalSolution( x );
136 *obj = single_qp.getObjVal( );
137 *status = getSimpleStatus( returnvalue );
138 *nWSRout = *nWSR;
139 single_qp.getDualSolution( y );
140
141 return;
142 }
143
144
145 /*
146 * Q P r o b l e m _ i n i t
147 */
sci_QProblem_init(real_t * H,real_t * g,real_t * A,real_t * lb,real_t * ub,real_t * lbA,real_t * ubA,int_t * nV,int_t * nC,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)148 void sci_QProblem_init( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
149 int_t* nV, int_t* nC, int_t* nWSR,
150 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
151 )
152 {
153 sci_QProblem_cleanup( );
154
155 /* transform A into C style matrix */
156 transformA( A, *nV,*nC );
157
158 /* setup and solve initial QP */
159 qp = new QProblem( *nV,*nC );
160 qp->setPrintLevel( PL_LOW );
161 returnValue returnvalue = qp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
162
163 /* assign lhs arguments */
164 qp->getPrimalSolution( x );
165 *obj = qp->getObjVal( );
166 *status = getSimpleStatus( returnvalue );
167 *nWSRout = *nWSR;
168 qp->getDualSolution( y );
169
170 return;
171 }
172
173
174 /*
175 * Q P r o b l e m B _ i n i t
176 */
sci_QProblemB_init(real_t * H,real_t * g,real_t * lb,real_t * ub,int_t * nV,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)177 void sci_QProblemB_init( real_t* H, real_t* g, real_t* lb, real_t* ub,
178 int_t* nV, int_t* nWSR,
179 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
180 )
181 {
182 sci_QProblemB_cleanup( );
183
184 /* setup and solve initial QP */
185 qpb = new QProblemB( *nV );
186 qpb->setPrintLevel( PL_LOW );
187 returnValue returnvalue = qpb->init( H,g,lb,ub, *nWSR,0 );
188
189 /* assign lhs arguments */
190 qpb->getPrimalSolution( x );
191 *obj = qpb->getObjVal( );
192 *status = getSimpleStatus( returnvalue );
193 *nWSRout = *nWSR;
194 qpb->getDualSolution( y );
195
196 return;
197 }
198
199
200 /*
201 * S Q P r o b l e m _ i n i t
202 */
sci_SQProblem_init(real_t * H,real_t * g,real_t * A,real_t * lb,real_t * ub,real_t * lbA,real_t * ubA,int_t * nV,int_t * nC,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)203 void sci_SQProblem_init( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
204 int_t* nV, int_t* nC, int_t* nWSR,
205 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
206 )
207 {
208 sci_SQProblem_cleanup( );
209
210 /* transform A into C style matrix */
211 transformA( A, *nV,*nC );
212
213 /* setup and solve initial QP */
214 sqp = new SQProblem( *nV,*nC );
215 sqp->setPrintLevel( PL_LOW );
216 returnValue returnvalue = sqp->init( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
217
218 /* assign lhs arguments */
219 sqp->getPrimalSolution( x );
220 *obj = sqp->getObjVal( );
221 *status = getSimpleStatus( returnvalue );
222 *nWSRout = *nWSR;
223 sqp->getDualSolution( y );
224
225 return;
226 }
227
228
229 /*
230 * Q P r o b l e m _ h o t s t a r t
231 */
sci_QProblem_hotstart(real_t * g,real_t * lb,real_t * ub,real_t * lbA,real_t * ubA,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)232 void sci_QProblem_hotstart( real_t* g, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
233 int_t* nWSR,
234 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
235 )
236 {
237 /* has QP been initialised? */
238 if ( qp == 0 )
239 {
240 *status = -1;
241 Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_init first!\n" );
242 return;
243 }
244
245 /* solve QP */
246 returnValue returnvalue = qp->hotstart( g,lb,ub,lbA,ubA, *nWSR,0 );
247
248 /* assign lhs arguments */
249 qp->getPrimalSolution( x );
250 *obj = qp->getObjVal( );
251 *status = getSimpleStatus( returnvalue );
252 *nWSRout = *nWSR;
253 qp->getDualSolution( y );
254
255 return;
256 }
257
258
259 /*
260 * Q P r o b l e m B _ h o t s t a r t
261 */
sci_QProblemB_hotstart(real_t * g,real_t * lb,real_t * ub,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)262 void sci_QProblemB_hotstart( real_t* g, real_t* lb, real_t* ub,
263 int_t* nWSR,
264 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
265 )
266 {
267 /* has QP been initialised? */
268 if ( qpb == 0 )
269 {
270 *status = -1;
271 Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_initSB first!\n" );
272 return;
273 }
274
275 /* solve QP */
276 returnValue returnvalue = qpb->hotstart( g,lb,ub, *nWSR,0 );
277
278 /* assign lhs arguments */
279 qpb->getPrimalSolution( x );
280 *obj = qpb->getObjVal( );
281 *status = getSimpleStatus( returnvalue );
282 *nWSRout = *nWSR;
283 qpb->getDualSolution( y );
284
285 return;
286 }
287
288
289 /*
290 * S Q P r o b l e m _ h o t s t a r t
291 */
sci_SQProblem_hotstart(real_t * H,real_t * g,real_t * A,real_t * lb,real_t * ub,real_t * lbA,real_t * ubA,int_t * nWSR,real_t * x,real_t * obj,int_t * status,int_t * nWSRout,real_t * y)292 void sci_SQProblem_hotstart( real_t* H, real_t* g, real_t* A, real_t* lb, real_t* ub, real_t* lbA, real_t* ubA,
293 int_t* nWSR,
294 real_t* x, real_t* obj, int_t* status, int_t* nWSRout, real_t* y
295 )
296 {
297 /* has QP been initialised? */
298 if ( sqp == 0 )
299 {
300 *status = -1;
301 Scierror( 999,"ERROR (qpOASES): Need to call qpOASES_initVM first!\n" );
302 return;
303 }
304
305 /* transform A into C style matrix */
306 transformA( A, sqp->getNV( ),sqp->getNC( ) );
307
308 /* solve QP */
309 returnValue returnvalue = sqp->hotstart( H,g,A,lb,ub,lbA,ubA, *nWSR,0 );
310
311 /* assign lhs arguments */
312 sqp->getPrimalSolution( x );
313 *obj = sqp->getObjVal( );
314 *status = getSimpleStatus( returnvalue );
315 *nWSRout = *nWSR;
316 sqp->getDualSolution( y );
317
318 return;
319 }
320
321
322 /*
323 * Q P r o b l e m _ c l e a n u p
324 */
sci_QProblem_cleanup()325 void sci_QProblem_cleanup( )
326 {
327 if ( qp != 0 )
328 {
329 delete qp;
330 qp = 0;
331 }
332
333 return;
334 }
335
336
337 /*
338 * Q P r o b l e m B _ c l e a n u p
339 */
sci_QProblemB_cleanup()340 void sci_QProblemB_cleanup( )
341 {
342 if ( qpb != 0 )
343 {
344 delete qpb;
345 qpb = 0;
346 }
347
348 return;
349 }
350
351
352 /*
353 * S Q P r o b l e m _ c l e a n u p
354 */
sci_SQProblem_cleanup()355 void sci_SQProblem_cleanup( )
356 {
357 if ( sqp != 0 )
358 {
359 delete sqp;
360 sqp = 0;
361 }
362
363 return;
364 }
365
366
367 /*
368 * end of file
369 */
370