eus_qpoases.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <qpOASES.hpp>
3 #include <map>
4 
5 using namespace qpOASES;
6 
7 // Solve QP/LP without hotstart, e.g., generate QProblem instance always.
8 double* solve_qpoases_qp_common (double* ret,
9  double* eval_weight_matrix, double* eval_coeff_vector,
10  double* state_min_vector, double* state_max_vector,
11  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
12  int state_len, int inequality_len, PrintLevel print_level, double* ret_status,
13  bool solve_lp) {
14  real_t* H = new real_t[state_len*state_len];
15  real_t* A = new real_t[inequality_len*state_len];
16  real_t* g = new real_t[state_len];
17  real_t* ub = new real_t[state_len];
18  real_t* lb = new real_t[state_len];
19  real_t* ubA = new real_t[inequality_len];
20  real_t* lbA = new real_t[inequality_len];
21  if (!solve_lp) {
22  for (int i = 0; i < state_len*state_len; i++) {
23  H[i] = eval_weight_matrix[i];
24  }
25  } else {
26  for (int i = 0; i < state_len*state_len; i++) {
27  H[i] = 0.0;
28  }
29  }
30  for (int i = 0; i < state_len; i++) {
31  ub[i] = state_max_vector[i];
32  }
33  for (int i = 0; i < state_len; i++) {
34  lb[i] = state_min_vector[i];
35  }
36  for (int i = 0; i < state_len; i++) {
37  g[i] = eval_coeff_vector[i];
38  }
39  for (int i = 0; i < inequality_len; i++) {
40  ubA[i] = inequality_max_vector[i];
41  }
42  for (int i = 0; i < inequality_len; i++) {
43  lbA[i] = inequality_min_vector[i];
44  }
45  for (int i = 0; i < inequality_len*state_len; i++) {
46  A[i] = inequality_matrix[i];
47  }
48 
49  QProblem example( state_len,inequality_len, (solve_lp ? HST_ZERO : HST_UNKNOWN));
50  Options options;
51  options.printLevel = print_level;
52  example.setOptions( options );
53  /* Solve first QP/LP. */
54  int nWSR = 10000;
55  returnValue status = example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
56  //printf("%d %d %d\n", print_level, PL_NONE, PL_MEDIUM);
57  ret_status[0] = getSimpleStatus(status, (print_level != PL_NONE)? BT_TRUE:BT_FALSE);
58  /* Get and print solution of second QP. */
59  real_t* xOpt = new real_t[state_len];
60  example.getPrimalSolution( xOpt );
61  for (int i = 0; i < state_len; i++) {
62  ret[i] = xOpt[i];
63  }
64  delete[] H;
65  delete[] A;
66  delete[] g;
67  delete[] ub;
68  delete[] lb;
69  delete[] ubA;
70  delete[] lbA;
71  delete[] xOpt;
72 
73  return ret;
74 }
75 
76 // Solve QP/LP with hotstart.
77 // QProblem instance are re-used.
78 // g, ub, lb, ubA, lbA can be updated.
79 // H and A should be constant.
80 std::map<std::pair<int, int>, QProblem*> qp_map;
81 double* solve_qpoases_qp_with_hotstart_common (double* ret,
82  double* eval_weight_matrix, double* eval_coeff_vector,
83  double* state_min_vector, double* state_max_vector,
84  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
85  int state_len, int inequality_len, PrintLevel print_level, double* ret_status,
86  bool solve_lp) {
87  real_t* H = new real_t[state_len*state_len];
88  real_t* A = new real_t[inequality_len*state_len];
89  real_t* g = new real_t[state_len];
90  real_t* ub = new real_t[state_len];
91  real_t* lb = new real_t[state_len];
92  real_t* ubA = new real_t[inequality_len];
93  real_t* lbA = new real_t[inequality_len];
94  if (!solve_lp) {
95  for (int i = 0; i < state_len*state_len; i++) {
96  H[i] = eval_weight_matrix[i];
97  }
98  } else {
99  for (int i = 0; i < state_len*state_len; i++) {
100  H[i] = 0.0;
101  }
102  }
103  for (int i = 0; i < state_len; i++) {
104  ub[i] = state_max_vector[i];
105  }
106  for (int i = 0; i < state_len; i++) {
107  lb[i] = state_min_vector[i];
108  }
109  for (int i = 0; i < state_len; i++) {
110  g[i] = eval_coeff_vector[i];
111  }
112  for (int i = 0; i < inequality_len; i++) {
113  ubA[i] = inequality_max_vector[i];
114  }
115  for (int i = 0; i < inequality_len; i++) {
116  lbA[i] = inequality_min_vector[i];
117  }
118  for (int i = 0; i < inequality_len*state_len; i++) {
119  A[i] = inequality_matrix[i];
120  }
121 
122  QProblem* example;
123  std::pair<int, int> tmp_pair(state_len, inequality_len);
124  std::map<std::pair<int, int>, QProblem*>::iterator it = qp_map.find(tmp_pair);
125  bool is_initial = (it == qp_map.end());
126  if (is_initial) {
127  example = new QProblem ( state_len,inequality_len, (solve_lp ? HST_ZERO : HST_UNKNOWN));
128  qp_map.insert(std::pair<std::pair<int, int>, QProblem*>(tmp_pair, example));
129  } else {
130  example = it->second;
131  }
132 
133  Options options;
134  options.printLevel = print_level;
135  example->setOptions( options );
136  int nWSR = 10000;
137  returnValue status;
138  if (is_initial) {
139  /* Solve first QP/LP. */
140  status = example->init( H,g,A,lb,ub,lbA,ubA, nWSR );
141  } else {
142  /* Solve second QP/LP. */
143  status = example->hotstart( g,lb,ub,lbA,ubA, nWSR );
144  }
145  //printf("qp [size = (%d,%d), pointer = %d, is_initial = %d, status = %d, solved = %d]\n", state_len, inequality_len, example, is_initial, status, example->isSolved());
146  //printf("%d %d %d\n", print_level, PL_NONE, PL_MEDIUM);
147  ret_status[0] = getSimpleStatus(status, (print_level != PL_NONE)? BT_TRUE:BT_FALSE);
148  if (example->isSolved() == BT_FALSE && ret_status[0] == 0) ret_status[0] = -100; // Not qpOASES original, check isSolved(). For example, hotstart.
149  /* Get and print solution of second QP/LP. */
150  real_t* xOpt = new real_t[state_len];
151  example->getPrimalSolution( xOpt );
152  for (int i = 0; i < state_len; i++) {
153  ret[i] = xOpt[i];
154  }
155  // Delete unsolved qp
156  if ( ret_status[0] != 0 ) {
157  delete qp_map[tmp_pair];
158  qp_map.erase(tmp_pair);
159  }
160 
161  delete[] H;
162  delete[] A;
163  delete[] g;
164  delete[] ub;
165  delete[] lb;
166  delete[] ubA;
167  delete[] lbA;
168  delete[] xOpt;
169 
170  return ret;
171 }
172 
173 // Solve Sequential QP/LP with hotstart.
174 // SQProblem instance are re-used.
175 // H, g, A, ub, lb, ubA, lbA can be updated.
176 std::map<std::pair<int, int>, SQProblem*> sqp_map;
177 double* solve_qpoases_sqp_with_hotstart_common (double* ret,
178  double* eval_weight_matrix, double* eval_coeff_vector,
179  double* state_min_vector, double* state_max_vector,
180  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
181  int state_len, int inequality_len, PrintLevel print_level, double* ret_status,
182  bool solve_lp) {
183  real_t* H = new real_t[state_len*state_len];
184  real_t* A = new real_t[inequality_len*state_len];
185  real_t* g = new real_t[state_len];
186  real_t* ub = new real_t[state_len];
187  real_t* lb = new real_t[state_len];
188  real_t* ubA = new real_t[inequality_len];
189  real_t* lbA = new real_t[inequality_len];
190  if (!solve_lp) {
191  for (int i = 0; i < state_len*state_len; i++) {
192  H[i] = eval_weight_matrix[i];
193  }
194  } else {
195  for (int i = 0; i < state_len*state_len; i++) {
196  H[i] = 0.0;
197  }
198  }
199  for (int i = 0; i < state_len; i++) {
200  ub[i] = state_max_vector[i];
201  }
202  for (int i = 0; i < state_len; i++) {
203  lb[i] = state_min_vector[i];
204  }
205  for (int i = 0; i < state_len; i++) {
206  g[i] = eval_coeff_vector[i];
207  }
208  for (int i = 0; i < inequality_len; i++) {
209  ubA[i] = inequality_max_vector[i];
210  }
211  for (int i = 0; i < inequality_len; i++) {
212  lbA[i] = inequality_min_vector[i];
213  }
214  for (int i = 0; i < inequality_len*state_len; i++) {
215  A[i] = inequality_matrix[i];
216  }
217 
218  SQProblem* example;
219  std::pair<int, int> tmp_pair(state_len, inequality_len);
220  std::map<std::pair<int, int>, SQProblem*>::iterator it = sqp_map.find(tmp_pair);
221  bool is_initial = (it == sqp_map.end());
222  if (is_initial) {
223  example = new SQProblem ( state_len,inequality_len, (solve_lp ? HST_ZERO : HST_UNKNOWN));
224  sqp_map.insert(std::pair<std::pair<int, int>, SQProblem*>(tmp_pair, example));
225  } else {
226  example = it->second;
227  }
228 
229  Options options;
230  options.printLevel = print_level;
231  example->setOptions( options );
232  int nWSR = 10000;
233  returnValue status;
234  if (is_initial) {
235  /* Solve first QP/LP. */
236  status = example->init( H,g,A,lb,ub,lbA,ubA, nWSR );
237  } else {
238  /* Solve second QP/LP. */
239  status = example->hotstart( H,g,A,lb,ub,lbA,ubA, nWSR );
240  }
241  //printf("sqp [size = (%d,%d), pointer = %d, is_initial = %d, status = %d, solved = %d]\n", state_len, inequality_len, example, is_initial, status, example->isSolved());
242  //printf("%d %d %d\n", print_level, PL_NONE, PL_MEDIUM);
243  ret_status[0] = getSimpleStatus(status, (print_level != PL_NONE)? BT_TRUE:BT_FALSE);
244  if (example->isSolved() == BT_FALSE && ret_status[0] == 0) ret_status[0] = -100; // Not qpOASES original, check isSolved(). For example, hotstart.
245  /* Get and print solution of second QP/LP. */
246  real_t* xOpt = new real_t[state_len];
247  example->getPrimalSolution( xOpt );
248  for (int i = 0; i < state_len; i++) {
249  ret[i] = xOpt[i];
250  }
251  // Delete unsolved sqp
252  if ( ret_status[0] != 0 ) {
253  delete sqp_map[tmp_pair];
254  sqp_map.erase(tmp_pair);
255  }
256 
257  delete[] H;
258  delete[] A;
259  delete[] g;
260  delete[] ub;
261  delete[] lb;
262  delete[] ubA;
263  delete[] lbA;
264  delete[] xOpt;
265 
266  return ret;
267 }
268 
269 extern "C" {
270  double* solve_qpoases_qp (double* ret,
271  double* eval_weight_matrix, double* eval_coeff_vector,
272  double* state_min_vector, double* state_max_vector,
273  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
274  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
275  solve_qpoases_qp_common(ret,
276  eval_weight_matrix, eval_coeff_vector,
277  state_min_vector, state_max_vector,
278  inequality_matrix, inequality_min_vector, inequality_max_vector,
279  state_len, inequality_len, print_level, ret_status, false);
280  };
281 
282  double* solve_qpoases_lp (double* ret,
283  double* eval_coeff_vector,
284  double* state_min_vector, double* state_max_vector,
285  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
286  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
287  solve_qpoases_qp_common(ret,
288  NULL, eval_coeff_vector,
289  state_min_vector, state_max_vector,
290  inequality_matrix, inequality_min_vector, inequality_max_vector,
291  state_len, inequality_len, print_level, ret_status, true);
292  };
293 
294  double* solve_qpoases_qp_with_hotstart (double* ret,
295  double* eval_weight_matrix, double* eval_coeff_vector,
296  double* state_min_vector, double* state_max_vector,
297  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
298  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
299  solve_qpoases_qp_with_hotstart_common(ret,
300  eval_weight_matrix, eval_coeff_vector,
301  state_min_vector, state_max_vector,
302  inequality_matrix, inequality_min_vector, inequality_max_vector,
303  state_len, inequality_len, print_level, ret_status, false);
304  };
305 
306  double* solve_qpoases_lp_with_hotstart (double* ret,
307  double* eval_coeff_vector,
308  double* state_min_vector, double* state_max_vector,
309  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
310  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
311  solve_qpoases_qp_with_hotstart_common(ret,
312  NULL, eval_coeff_vector,
313  state_min_vector, state_max_vector,
314  inequality_matrix, inequality_min_vector, inequality_max_vector,
315  state_len, inequality_len, print_level, ret_status, true);
316  };
317 
318  double* solve_qpoases_sqp_with_hotstart (double* ret,
319  double* eval_weight_matrix, double* eval_coeff_vector,
320  double* state_min_vector, double* state_max_vector,
321  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
322  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
323  solve_qpoases_sqp_with_hotstart_common(ret,
324  eval_weight_matrix, eval_coeff_vector,
325  state_min_vector, state_max_vector,
326  inequality_matrix, inequality_min_vector, inequality_max_vector,
327  state_len, inequality_len, print_level, ret_status, false);
328  };
329 
330  double* solve_qpoases_slp_with_hotstart (double* ret,
331  double* eval_coeff_vector,
332  double* state_min_vector, double* state_max_vector,
333  double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
334  int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
335  solve_qpoases_sqp_with_hotstart_common(ret,
336  NULL, eval_coeff_vector,
337  state_min_vector, state_max_vector,
338  inequality_matrix, inequality_min_vector, inequality_max_vector,
339  state_len, inequality_len, print_level, ret_status, true);
340  };
341 }
solve_qpoases_slp_with_hotstart
double * solve_qpoases_slp_with_hotstart(double *ret, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:330
solve_qpoases_qp_common
double * solve_qpoases_qp_common(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status, bool solve_lp)
Definition: eus_qpoases.cpp:8
solve_qpoases_sqp_with_hotstart_common
double * solve_qpoases_sqp_with_hotstart_common(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status, bool solve_lp)
Definition: eus_qpoases.cpp:177
sqp_map
std::map< std::pair< int, int >, SQProblem * > sqp_map
Definition: eus_qpoases.cpp:176
solve_qpoases_qp_with_hotstart_common
double * solve_qpoases_qp_with_hotstart_common(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status, bool solve_lp)
Definition: eus_qpoases.cpp:81
solve_qpoases_qp_with_hotstart
double * solve_qpoases_qp_with_hotstart(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:294
solve_qpoases_lp_with_hotstart
double * solve_qpoases_lp_with_hotstart(double *ret, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:306
qp_map
std::map< std::pair< int, int >, QProblem * > qp_map
Definition: eus_qpoases.cpp:80
A
A
NULL
#define NULL
solve_qpoases_qp
double * solve_qpoases_qp(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:270
solve_qpoases_lp
double * solve_qpoases_lp(double *ret, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:282
solve_qpoases_sqp_with_hotstart
double * solve_qpoases_sqp_with_hotstart(double *ret, double *eval_weight_matrix, double *eval_coeff_vector, double *state_min_vector, double *state_max_vector, double *inequality_matrix, double *inequality_min_vector, double *inequality_max_vector, int state_len, int inequality_len, PrintLevel print_level, double *ret_status)
Definition: eus_qpoases.cpp:318

eus_qpoases
Author(s):
autogenerated on Fri May 14 2021 02:51:42