dense_cp.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
33 #include <acado/conic_program/dense_cp.hpp>
34 
35 using namespace std;
36 
37 BEGIN_NAMESPACE_ACADO
38 
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
44 DenseCP::DenseCP( ){
45 
46  nS = 0;
47 
48  B = 0;
49  lbB = 0;
50  ubB = 0;
51 
52  x = 0;
53  ylb = 0;
54  yub = 0;
55 
56  ylbA = 0;
57  yubA = 0;
58 
59  ylbB = 0;
60  yubB = 0;
61 }
62 
63 
64 DenseCP::DenseCP( const DenseCP& rhs ){
65 
66  copy(rhs);
67 }
68 
69 
70 DenseCP::~DenseCP( ){
71 
72  clean();
73 }
74 
75 
76 DenseCP& DenseCP::operator=( const DenseCP& rhs ){
77 
78  if ( this != &rhs ){
79 
80  clean() ;
81  copy(rhs);
82  }
83  return *this;
84 }
85 
86 
87 
88 void DenseCP::copy( const DenseCP& rhs ){
89 
90  uint run1, run2;
91 
92  nS = rhs.nS;
93 
94  H = rhs.H;
95  g = rhs.g;
96 
97  lb = rhs.lb;
98  ub = rhs.ub;
99 
100  A = rhs.A;
101  lbA = rhs.lbA;
102  ubA = rhs.ubA;
103 
104 
105  if( rhs.B != 0 ){
106  B = (DMatrix**)calloc(nS,sizeof(DMatrix*));
107  for( run1 = 0; run1 < nS; run1++ ){
108  B[run1] = new DMatrix[ getNV() ];
109  for( run2 = 0; run2 < getNV(); run2++ )
110  B[run1][run2] = rhs.B[run1][run2];
111  }
112  }
113  else B = 0;
114 
115 
116  if( rhs.lbB != 0 ){
117  lbB = (DVector*)calloc(nS,sizeof(DVector));
118  for( run1 = 0; run1 < nS; run1++ )
119  lbB[run1] = rhs.lbB[run1];
120  }
121  else lbB = 0;
122 
123  if( rhs.ubB != 0 ){
124  ubB = (DVector*)calloc(nS,sizeof(DVector));
125  for( run1 = 0; run1 < nS; run1++ )
126  ubB[run1] = rhs.ubB[run1];
127  }
128  else ubB = 0;
129 
130 
131  if( rhs.x != 0 ) x = new DVector(*rhs.x);
132  else x = 0 ;
133 
134  if( rhs.ylb != 0 ) ylb = new DVector(*rhs.ylb);
135  else ylb = 0 ;
136 
137  if( rhs.yub != 0 ) yub = new DVector(*rhs.yub);
138  else yub = 0 ;
139 
140  if( rhs.ylbA != 0 ) ylbA = new DVector(*rhs.ylbA);
141  else ylbA = 0 ;
142 
143  if( rhs.yubA != 0 ) yubA = new DVector(*rhs.yubA);
144  else yubA = 0 ;
145 
146  if( nS > 0 ){
147 
148  ylbB = (DVector**)calloc(nS,sizeof(DVector*));
149  yubB = (DVector**)calloc(nS,sizeof(DVector*));
150 
151  for( run1 = 0; run1 < nS; run1++ ){
152  if( rhs.ylbB[run1] != 0 ) ylbB[run1] = new DVector(*rhs.ylbB[run1]);
153  else ylbB[run1] = 0 ;
154 
155  if( rhs.yubB[run1] != 0 ) yubB[run1] = new DVector(*rhs.yubB[run1]);
156  else yubB[run1] = 0 ;
157  }
158  }
159  else{
160  ylbB = 0;
161  yubB = 0;
162  }
163 }
164 
165 
166 
167 void DenseCP::clean(){
168 
169  uint run1;
170 
171  for( run1 = 0; run1 < nS; run1++ ){
172 
173  if( B[run1] != 0 ) delete[] B[run1];
174  if( ylbB[run1] != 0 ) delete ylbB[run1];
175  if( yubB[run1] != 0 ) delete yubB[run1];
176  }
177 
178  if( B != 0 ) free(B) ;
179  if( ylbB != 0 ) free(ylbB);
180  if( yubB != 0 ) free(yubB);
181 
182  if( lbB != 0 ) free(lbB);
183  if( ubB != 0 ) free(ubB);
184 
185  if( ylbA != 0 ) delete ylbA;
186  if( yubA != 0 ) delete yubA;
187 
188  if( ylb != 0 ) delete ylb;
189  if( yub != 0 ) delete yub;
190 
191  if( x != 0 ) delete x;
192 }
193 
194 
195 returnValue DenseCP::init( uint nV_, uint nC_ ){
196 
197  H.init(nV_,nV_);
198  A.init(nC_,nV_);
199 
200  return SUCCESSFUL_RETURN;
201 }
202 
203 
204 returnValue DenseCP::setQPsolution( const DVector &x_, const DVector &y_ ){
205 
206  uint run1;
207  clean();
208 
209  ASSERT( x_.getDim() == getNV() );
210  ASSERT( y_.getDim() == getNV() + getNC() );
211 
212 
213  // SET THE PRIMAL SOLUTION:
214  // ------------------------
215  x = new DVector(x_);
216 
217 
218  // SET THE DUAL SOLUTION FOR THE BOUNDS:
219  // -------------------------------------
220  ylb = new DVector( getNV() );
221  yub = new DVector( getNV() );
222 
223  for( run1 = 0; run1 < getNV(); run1++ ){
224  if( fabs(x_(run1)-lb(run1)) <= BOUNDTOL ){
225  ylb->operator()(run1) = y_(run1);
226  yub->operator()(run1) = 0.0 ;
227  }
228  else{
229  ylb->operator()(run1) = 0.0 ;
230  yub->operator()(run1) = y_(run1);
231  }
232  }
233 
234 
235  // SET THE DUAL SOLUTION FOR THE CONSTRAINTS:
236  // ------------------------------------------
237  DVector tmp = A*x_;
238  ylbA = new DVector( getNC() );
239  yubA = new DVector( getNC() );
240 
241  for( run1 = 0; run1 < getNC(); run1++ ){
242  if( fabs(tmp(run1)-lbA(run1)) <= BOUNDTOL ){
243  ylbA->operator()(run1) = y_(getNV()+run1);
244  yubA->operator()(run1) = 0.0 ;
245  }
246  else{
247  ylbA->operator()(run1) = 0.0 ;
248  yubA->operator()(run1) = y_(getNV()+run1);
249  }
250  }
251 
252 
253  return SUCCESSFUL_RETURN;
254 }
255 
256 
257 DVector DenseCP::getMergedDualSolution( ) const
258 {
259  DVector dualSolution( getNV()+getNC() );
260 
261  uint i;
262  for( i=0; i<getNV(); ++i )
263  {
264  if ( acadoIsGreater( fabs( (*ylb)(i) ),1e-10 ) == BT_TRUE )
265  dualSolution(i) = (*ylb)(i);
266  else
267  dualSolution(i) = (*yub)(i);
268  }
269 
270  for( i=0; i<getNC(); ++i )
271  {
272  if ( acadoIsGreater( fabs( (*ylbA)(i) ),1e-10 ) == BT_TRUE )
273  dualSolution( getNV()+i ) = (*ylbA)(i);
274  else
275  dualSolution( getNV()+i ) = (*yubA)(i);
276  }
277 
278  return dualSolution;
279 }
280 
281 
282 
283 returnValue DenseCP::print( const char* const name,
284  const char* const startString,
285  const char* const endString,
286  uint width,
287  uint precision,
288  const char* const colSeparator,
289  const char* const rowSeparator
290  ) const
291 {
292  H.print (cout, "H", startString,endString,width,precision,colSeparator,rowSeparator );
293  g.print (cout, "g", startString,endString,width,precision,colSeparator,rowSeparator );
294  lb.print (cout, "lb", startString,endString,width,precision,colSeparator,rowSeparator );
295  ub.print (cout, "ub", startString,endString,width,precision,colSeparator,rowSeparator );
296  A.print (cout, "A", startString,endString,width,precision,colSeparator,rowSeparator );
297  lbA.print(cout, "lbA",startString,endString,width,precision,colSeparator,rowSeparator );
298  ubA.print(cout, "ubA",startString,endString,width,precision,colSeparator,rowSeparator );
299 
300  return SUCCESSFUL_RETURN;
301 }
302 
303 
304 returnValue DenseCP::print( const char* const name,
305  PrintScheme printScheme
306  ) const
307 {
308  H.print (cout, "H", printScheme );
309  g.print (cout, "g", printScheme );
310  lb.print (cout, "lb", printScheme );
311  ub.print (cout, "ub", printScheme );
312  A.print (cout, "A", printScheme );
313  lbA.print(cout, "lbA",printScheme );
314  ubA.print(cout, "ubA",printScheme );
315 
316  return SUCCESSFUL_RETURN;
317 }
318 
319 
320 returnValue DenseCP::printToFile( const char* const filename,
321  const char* const name,
322  const char* const startString,
323  const char* const endString,
324  uint width,
325  uint precision,
326  const char* const colSeparator,
327  const char* const rowSeparator
328  ) const
329 {
330  H.print ( filename,"H", startString,endString,width,precision,colSeparator,rowSeparator );
331  g.print ( filename,"g", startString,endString,width,precision,colSeparator,rowSeparator );
332  lb.print ( filename,"lb", startString,endString,width,precision,colSeparator,rowSeparator );
333  ub.print ( filename,"ub", startString,endString,width,precision,colSeparator,rowSeparator );
334  A.print ( filename,"A", startString,endString,width,precision,colSeparator,rowSeparator );
335  lbA.print( filename,"lbA",startString,endString,width,precision,colSeparator,rowSeparator );
336  ubA.print( filename,"ubA",startString,endString,width,precision,colSeparator,rowSeparator );
337 
338  return SUCCESSFUL_RETURN;
339 }
340 
341 
342 returnValue DenseCP::printToFile( std::ostream& stream,
343  const char* const name,
344  const char* const startString,
345  const char* const endString,
346  uint width,
347  uint precision,
348  const char* const colSeparator,
349  const char* const rowSeparator
350  ) const
351 {
352  H.print ( stream,"H", startString,endString,width,precision,colSeparator,rowSeparator );
353  g.print ( stream,"g", startString,endString,width,precision,colSeparator,rowSeparator );
354  lb.print ( stream,"lb", startString,endString,width,precision,colSeparator,rowSeparator );
355  ub.print ( stream,"ub", startString,endString,width,precision,colSeparator,rowSeparator );
356  A.print ( stream,"A", startString,endString,width,precision,colSeparator,rowSeparator );
357  lbA.print( stream,"lbA",startString,endString,width,precision,colSeparator,rowSeparator );
358  ubA.print( stream,"ubA",startString,endString,width,precision,colSeparator,rowSeparator );
359 
360  return SUCCESSFUL_RETURN;
361 }
362 
363 
364 returnValue DenseCP::printToFile( const char* const filename,
365  const char* const name,
366  PrintScheme printScheme
367  ) const
368 {
369  H.print ( filename,"H", printScheme );
370  g.print ( filename,"g", printScheme );
371  lb.print ( filename,"lb", printScheme );
372  ub.print ( filename,"ub", printScheme );
373  A.print ( filename,"A", printScheme );
374  lbA.print( filename,"lbA",printScheme );
375  ubA.print( filename,"ubA",printScheme );
376 
377  return SUCCESSFUL_RETURN;
378 }
379 
380 
381 returnValue DenseCP::printToFile( std::ostream& stream,
382  const char* const name,
383  PrintScheme printScheme
384  ) const
385 {
386  H.print ( stream,"H", printScheme );
387  g.print ( stream,"g", printScheme );
388  lb.print ( stream,"lb", printScheme );
389  ub.print ( stream,"ub", printScheme );
390  A.print ( stream,"A", printScheme );
391  lbA.print( stream,"lbA",printScheme );
392  ubA.print( stream,"ubA",printScheme );
393 
394  return SUCCESSFUL_RETURN;
395 }
396 
397 
398 
399 returnValue DenseCP::printSolution( const char* const name,
400  const char* const startString,
401  const char* const endString,
402  uint width,
403  uint precision,
404  const char* const colSeparator,
405  const char* const rowSeparator
406  ) const
407 {
408  if ( x != 0 )
409  x->print(cout, "x", startString,endString,width,precision,colSeparator,rowSeparator );
410 
411  if ( ylb != 0 )
412  ylb->print(cout, "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
413 
414  if ( yub != 0 )
415  yub->print(cout, "yub", startString,endString,width,precision,colSeparator,rowSeparator );
416 
417  if ( ylbA != 0 )
418  ylbA->print(cout, "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
419 
420  if ( yubA != 0 )
421  yubA->print(cout, "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
422 
423  return SUCCESSFUL_RETURN;
424 }
425 
426 
427 returnValue DenseCP::printSolution( const char* const name,
428  PrintScheme printScheme
429  ) const
430 {
431  if ( x != 0 )
432  x->print(cout, "x", printScheme );
433 
434  if ( ylb != 0 )
435  ylb->print(cout, "ylb",printScheme );
436 
437  if ( yub != 0 )
438  yub->print(cout, "yub", printScheme );
439 
440  if ( ylbA != 0 )
441  ylbA->print(cout, "ylbA", printScheme );
442 
443  if ( yubA != 0 )
444  yubA->print(cout, "yubA", printScheme );
445 
446  return SUCCESSFUL_RETURN;
447 }
448 
449 
450 returnValue DenseCP::printSolutionToFile( const char* const filename,
451  const char* const name,
452  const char* const startString,
453  const char* const endString,
454  uint width,
455  uint precision,
456  const char* const colSeparator,
457  const char* const rowSeparator
458  ) const
459 {
460  if ( x != 0 )
461  x->print( filename, "x", startString,endString,width,precision,colSeparator,rowSeparator );
462 
463  if ( ylb != 0 )
464  ylb->print( filename, "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
465 
466  if ( yub != 0 )
467  yub->print( filename, "yub", startString,endString,width,precision,colSeparator,rowSeparator );
468 
469  if ( ylbA != 0 )
470  ylbA->print( filename, "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
471 
472  if ( yubA != 0 )
473  yubA->print( filename, "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
474 
475  return SUCCESSFUL_RETURN;
476 }
477 
478 
479 returnValue DenseCP::printSolutionToFile( std::ostream& stream,
480  const char* const name,
481  const char* const startString,
482  const char* const endString,
483  uint width,
484  uint precision,
485  const char* const colSeparator,
486  const char* const rowSeparator
487  ) const
488 {
489  if ( x != 0 )
490  x->print( stream, "x", startString,endString,width,precision,colSeparator,rowSeparator );
491 
492  if ( ylb != 0 )
493  ylb->print( stream, "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
494 
495  if ( yub != 0 )
496  yub->print( stream, "yub", startString,endString,width,precision,colSeparator,rowSeparator );
497 
498  if ( ylbA != 0 )
499  ylbA->print( stream, "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
500 
501  if ( yubA != 0 )
502  yubA->print( stream, "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
503 
504  return SUCCESSFUL_RETURN;
505 }
506 
507 
508 returnValue DenseCP::printSolutionToFile( const char* const filename,
509  const char* const name,
510  PrintScheme printScheme
511  ) const
512 {
513  if ( x != 0 )
514  x->print( filename, "x", printScheme );
515 
516  if ( ylb != 0 )
517  ylb->print( filename, "ylb",printScheme );
518 
519  if ( yub != 0 )
520  yub->print( filename, "yub", printScheme );
521 
522  if ( ylbA != 0 )
523  ylbA->print( filename, "ylbA", printScheme );
524 
525  if ( yubA != 0 )
526  yubA->print( filename, "yubA", printScheme );
527 
528  return SUCCESSFUL_RETURN;
529 }
530 
531 
532 returnValue DenseCP::printSolutionToFile( std::ostream& stream,
533  const char* const name,
534  PrintScheme printScheme
535  ) const
536 {
537  if ( x != 0 )
538  x->print( stream, "x", printScheme );
539 
540  if ( ylb != 0 )
541  ylb->print( stream, "ylb",printScheme );
542 
543  if ( yub != 0 )
544  yub->print( stream, "yub", printScheme );
545 
546  if ( ylbA != 0 )
547  ylbA->print( stream, "ylbA", printScheme );
548 
549  if ( yubA != 0 )
550  yubA->print( stream, "yubA", printScheme );
551 
552  return SUCCESSFUL_RETURN;
553 }
554 
555 
556 
557 CLOSE_NAMESPACE_ACADO
558 
559 // end of file.
DenseCP::lb
DVector lb
Definition: dense_cp.hpp:384
DenseCP::DenseCP
DenseCP()
Definition: dense_cp.cpp:44
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
DenseCP::ub
DVector ub
Definition: dense_cp.hpp:385
DenseCP::operator=
DenseCP & operator=(const DenseCP &rhs)
Definition: dense_cp.cpp:76
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
DenseCP::g
DVector g
Definition: dense_cp.hpp:382
DenseCP::print
returnValue print(const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: dense_cp.cpp:283
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
DenseCP::setQPsolution
returnValue setQPsolution(const DVector &x_, const DVector &y_)
Definition: dense_cp.cpp:204
DenseCP::yub
DVector * yub
Definition: dense_cp.hpp:401
acadoIsGreater
BooleanType acadoIsGreater(double x, double y, double TOL)
Definition: acado_utils.cpp:106
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
BOUNDTOL
const double BOUNDTOL
Definition: acado_constants.hpp:79
DenseCP::B
DMatrix ** B
Definition: dense_cp.hpp:391
DenseCP::yubB
DVector ** yubB
Definition: dense_cp.hpp:407
DenseCP::H
DMatrix H
Definition: dense_cp.hpp:381
DenseCP::yubA
DVector * yubA
Definition: dense_cp.hpp:404
DenseCP::clean
void clean()
Definition: dense_cp.cpp:167
DenseCP::getMergedDualSolution
DVector getMergedDualSolution() const
Definition: dense_cp.cpp:257
DenseCP
Data class for storing generic conic programs.
Definition: dense_cp.hpp:55
DenseCP::ylbA
DVector * ylbA
Definition: dense_cp.hpp:403
DenseCP::~DenseCP
virtual ~DenseCP()
Definition: dense_cp.cpp:70
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
DenseCP::nS
uint nS
Definition: dense_cp.hpp:375
PrintScheme
PrintScheme
Definition: acado_types.hpp:574
DenseCP::copy
void copy(const DenseCP &rhs)
Definition: dense_cp.cpp:88
DenseCP::x
DVector * x
Definition: dense_cp.hpp:398
DenseCP::lbB
DVector * lbB
Definition: dense_cp.hpp:392
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
DenseCP::ubA
DVector ubA
Definition: dense_cp.hpp:389
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
DVector
GenericVector< double > DVector
Definition: vector.hpp:329
DenseCP::A
DMatrix A
Definition: dense_cp.hpp:387
dense_cp.hpp
DenseCP::printToFile
returnValue printToFile(const char *const filename, const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: dense_cp.cpp:320
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
DenseCP::lbA
DVector lbA
Definition: dense_cp.hpp:388
DenseCP::ylb
DVector * ylb
Definition: dense_cp.hpp:400
DenseCP::printSolutionToFile
returnValue printSolutionToFile(const char *const filename, const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: dense_cp.cpp:450
DenseCP::ylbB
DVector ** ylbB
Definition: dense_cp.hpp:406
DenseCP::printSolution
returnValue printSolution(const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: dense_cp.cpp:399
DenseCP::init
returnValue init(uint nV_, uint nC_)
Definition: dense_cp.cpp:195
DenseCP::ubB
DVector * ubB
Definition: dense_cp.hpp:393

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