qpOASES-3.2.0/examples/example5.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2015 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 
37 #include <stdlib.h>
38 
39 #include <qpOASES.hpp>
40 #include "example4CP.cpp"
41 
42 
45 int main( )
46 {
48 
49  int_t i,j,jj;
50  real_t d = 0.0;
51 
52  /* Setup data of first QP... */
53  real_t H[7*7];
54  real_t A[50*7];
55  real_t g[7];
56  real_t lbA[50];
57 
58  /* ( 1.0 0.5 | )
59  * ( 0.5 2.0 | )
60  * ( --------+------------------- )
61  * H = ( | 1e-6 )
62  * ( | 1e-6 )
63  * ( | ... )
64  * ( | 1e-6 ) */
65  for( i=0; i<7*7; ++i )
66  H[i] = 0.0;
67  for( i=2; i<7; ++i )
68  H[i*7+i] = 1.0e-6;
69  H[0] = 1.0;
70  H[1] = 0.5;
71  H[7] = 0.5;
72  H[8] = 2.0;
73 
74  /* ( x.x x.x | 1.0 )
75  * ( x.x x.x | ... )
76  * ( x.x x.x | 1.0 )
77  * ( x.x x.x | 1.0 )
78  * A = ( x.x x.x | ... )
79  * ( x.x x.x | 1.0 )
80  * ( x.x x.x | ... )
81  * ( x.x x.x | 1.0 )
82  * ( x.x x.x | ... )
83  * ( x.x x.x | 1.0 ) */
84  for( i=0; i<50*7; ++i )
85  A[i] = 0.0;
86  for( i=0; i<50; ++i )
87  {
88  for( j=0; j<2; ++j )
89  A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;
90 
91  A[i*7 + (i/10)+2] = 1.0;
92  }
93 
94  /* ( -1.0 )
95  * ( -0.5 )
96  * ( ---- )
97  * g = ( )
98  * ( )
99  * ( )
100  * ( ) */
101  for( i=0; i<7; ++i )
102  g[i] = 0.0;
103  g[0] = -1.0;
104  g[1] = -0.5;
105 
106  for( i=0; i<50; ++i )
107  lbA[i] = 1.0;
108 
109  /* ... and setting up user-defined constraint product function. */
110  MyConstraintProduct myCP( 7,50,A );
111 
112 
113  /* Setting up QProblem object and set construct product function. */
114  QProblem example( 7,50 );
115  example.setConstraintProduct( &myCP );
116 
117 
118  /* Solve first QP. */
119  real_t cputime = 1.0;
120  int_t nWSR = 100;
121  example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );
122 
123  /* Get and print solution of QP. */
124  real_t xOpt[7], yOpt[7+50];
125  example.getPrimalSolution( xOpt );
126  example.getDualSolution( yOpt );
127 
128 
129  /* Compute local linear feedback law */
130  const int_t n_rhs = 7+7+50;
131  real_t g_in[7*n_rhs];
132  real_t b_in[7*n_rhs];
133  real_t bA_in[50*n_rhs];
134  real_t x_out[7*n_rhs];
135  real_t y_out[(7+50)*n_rhs];
136 
137  int_t ii;
138  memset (g_in, 0, sizeof (g_in));
139  memset (b_in, 0, sizeof (b_in));
140  memset (bA_in, 0, sizeof (bA_in));
141 
142  for ( ii = 0; ii < 7; ++ii )
143  g_in[ii*7 + ii] = 1.0;
144  for ( ii = 0; ii < 7; ++ii )
145  b_in[(ii+7)*7 + ii] = 1.0;
146  for ( ii = 0; ii < 50; ++ii )
147  bA_in[(ii+14)*50 + ii] = 1.0;
148 
149  example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );
150 
151  /* Verify validity of local feedback law by perturbation and hot starts */
152  real_t perturb = 1.0e-6;
153  real_t nrm = 0.0;
154  for ( ii = 0; ii < n_rhs; ++ii )
155  {
156  for ( jj = 0; jj < 7; ++jj )
157  g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
158  for ( jj = 0; jj < 50; ++jj )
159  bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;
160 
161  nWSR = 100;
162  example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );
163 
164  real_t xPer[7], yPer[7+50];
165  example.getPrimalSolution( xPer );
166  example.getDualSolution( yPer );
167 
168  for ( jj = 0; jj < 7; ++jj )
169  {
170  d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
171  if (nrm < d) nrm=d;
172  }
173  for ( jj = 0; jj < 7+50; ++jj )
174  {
175  d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
176  if (nrm < d) nrm=d;
177  }
178  }
179  printf ("Maximum perturbation over all directions: %e\n", nrm);
180 
181  /* // print feedback matrix
182  for (ii = 0; ii < n_rhs; ++ii)
183  {
184  printf ("x: ");
185  for (jj = 0; jj < 7; ++jj )
186  printf ("%8.2e ", x_out[ii*7+jj]);
187  printf (" y: ");
188  for (jj = 0; jj < 7+50; ++jj )
189  printf ("%8.2e ", y_out[ii*(7+50)+jj]);
190  printf("\n");
191  }
192 */
193 
194  return 0;
195 }
196 
197 
198 /*
199  * end of file
200  */
returnValue solveCurrentEQP(const int n_rhs, const real_t *g_in, const real_t *lb_in, const real_t *ub_in, const real_t *lbA_in, const real_t *ubA_in, real_t *x_out, real_t *y_out)
returnValue init(const real_t *const _H, const real_t *const _g, const real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int &nWSR, const real_t *const yOpt=0, real_t *const cputime=0)
real_t getAbs(real_t x)
returnValue hotstart(const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int &nWSR, real_t *const cputime)
Example illustrating the use of the ConstraintProduct class.
returnValue setConstraintProduct(ConstraintProduct *const _constraintProduct)
double real_t
Definition: AD_test.c:10
Implements the online active set strategy for QPs with general constraints.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:33