curve.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/curve/curve.hpp>
34 
35 
36 
38 
39 
40 
41 //
42 // PUBLIC MEMBER FUNCTIONS:
43 //
44 
46 
47  nIntervals = 0;
48  dim = 0;
49  parameterization = 0;
50  grid = 0;
51 }
52 
53 Curve::Curve( const Curve& arg ){
54 
55  uint run1;
56 
57  nIntervals = arg.nIntervals;
58  dim = arg.dim ;
59 
60  parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
61 
62  for( run1 = 0; run1 < nIntervals; run1++ ){
63  if( arg.parameterization[run1] != 0 ) parameterization[run1] = new Function(*arg.parameterization[run1]);
64  else parameterization[run1] = 0;
65  }
66 
67  if( arg.grid != 0 ) grid = new Grid(*arg.grid);
68  else grid = 0;
69 }
70 
71 
73 
74  uint run1;
75 
76  for( run1 = 0; run1 < nIntervals; run1++ )
77  if( parameterization[run1] != 0 )
78  delete parameterization[run1];
79 
80  if( parameterization != 0 ) free(parameterization);
81  if( grid != 0 ) delete grid;
82 }
83 
84 
85 Curve& Curve::operator=( const Curve& arg ){
86 
87  uint run1;
88 
89  if ( this != &arg ){
90 
91  for( run1 = 0; run1 < nIntervals; run1++ )
92  if( parameterization[run1] != 0 )
93  delete parameterization[run1];
94 
95  if( parameterization != 0 ) free(parameterization);
96  if( grid != 0 ) delete grid;
97 
98  nIntervals = arg.nIntervals;
99  dim = arg.dim ;
100  parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
101 
102  for( run1 = 0; run1 < nIntervals; run1++ ){
103  if( arg.parameterization[run1] != 0 ) parameterization[run1] = new Function(*arg.parameterization[run1]);
104  else parameterization[run1] = 0;
105  }
106 
107  if( arg.grid != 0 ) grid = new Grid(*arg.grid);
108  else grid = 0;
109  }
110  return *this;
111 }
112 
113 
115  ) const
116 {
117  Curve tmp;
118 
119  if ( (int)idx >= getDim() )
120  {
122  return tmp;
123  }
124 
125  uint run1;
126 
127  tmp.nIntervals = nIntervals;
128  tmp.dim = dim ; // -> 1!
129  tmp.parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
130 
131  for( run1 = 0; run1 < nIntervals; run1++ ){
132  if( parameterization[run1] != 0 ) tmp.parameterization[run1] = new Function( parameterization[run1]->operator()( idx ) );
133  else tmp.parameterization[run1] = 0;
134  }
135 
136  if( grid != 0 ) tmp.grid = new Grid(*grid);
137  else tmp.grid = 0;
138 
139  return tmp;
140 }
141 
142 
143 returnValue Curve::add( double tStart, double tEnd, const DVector constant ){
144 
145  uint run1;
146  Function tmp ;
147 
148  // CONSTRUCT SIMPLY A CONSTANT FUNCTION AND ADD IT:
149  // ------------------------------------------------
150 
151  for( run1 = 0; run1 < constant.getDim(); run1++ )
152  tmp << constant(run1);
153 
154  return add( tStart, tEnd, tmp );
155 }
156 
157 
159 
160  uint run1,run2 ;
161  returnValue returnvalue;
162  Function *tmp ;
163  TIME t ;
164  double m,b ;
165 
166  switch( mode ){
167 
168  case IM_CONSTANT:
169  for( run1 = 0; run1 < sampledData.getNumPoints()-1; run1++ ){
170  tmp = new Function();
171  for( run2 = 0; run2 < sampledData.getNumRows(); run2++ )
172  tmp->operator<<( sampledData(run1,run2) );
173  returnvalue = add( sampledData.getTime(run1), sampledData.getTime(run1+1), *tmp );
174  if( returnvalue != SUCCESSFUL_RETURN )
175  ACADOERROR(returnvalue);
176  delete tmp;
177  }
178  return SUCCESSFUL_RETURN;
179 
180 
181  case IM_LINEAR:
182  for( run1 = 0; run1 < sampledData.getNumPoints()-1; run1++ ){
183  tmp = new Function();
184  for( run2 = 0; run2 < sampledData.getNumRows(); run2++ ){
185 
186  m = (sampledData(run1+1,run2) - sampledData(run1,run2) )/
187  (sampledData.getTime(run1+1) - sampledData.getTime(run1));
188 
189  b = sampledData(run1,run2) - m*sampledData.getTime(run1);
190 
191  tmp->operator<<( m*t + b );
192  }
193  returnvalue = add( sampledData.getTime(run1), sampledData.getTime(run1+1), *tmp );
194  if( returnvalue != SUCCESSFUL_RETURN )
195  ACADOERROR(returnvalue);
196  delete tmp;
197  }
198  return SUCCESSFUL_RETURN;
199 
200 
201  case IM_QUADRATIC:
203 
204  case IM_CUBIC:
206 
207  default:
209  }
210  return SUCCESSFUL_RETURN;
211 }
212 
213 
214 
215 returnValue Curve::add( double tStart, double tEnd, const Function &parameterization_ ){
216 
217  uint run1;
218 
219  // CHECK WHETHER "tStart < tEnd":
220  // ------------------------------------------------
221  if( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE )
223 
224 
225  // CHECK WHETHER THE INPUT VECTOR IS EMPTY:
226  // ------------------------------------------------
227  if( parameterization_.getDim() == 0 )
229 
230 
231 
232  if( isEmpty() == BT_FALSE ){
233 
234  // IF THE CURVE IS NOT EMPTY THE DIMENSIONS MUST BE CHECKED:
235  // ---------------------------------------------------------
236  if( getDim() != (int) parameterization_.getDim() )
238 
239  // CHECK WHETHER THE CURVE HAS NO GAP's:
240  // ---------------------------------------------------------
241  if( acadoIsEqual( tStart,grid->getLastTime() ) == BT_FALSE )
242  {
243  ASSERT(1==0);
245  }
246 
247  // APPEND THE NEW TIME INTERVAL TO THE GRID:
248  // ---------------------------------------------------------
249 
250  double *times = new double[nIntervals+2];
251  for( run1 = 0; run1 < nIntervals+1; run1++ )
252  times[run1] = grid->getTime(run1);
253  times[nIntervals+1] = tEnd;
254 
255  delete grid;
256  grid = new Grid( nIntervals+2, times );
257 
258  nIntervals++;
259  delete[] times;
260 
261  // ---------------------------------------------------------
262  }
263  else{
264 
265  // SETUP A NEW GRID:
266  // ----------------------------------------------------
267 
268  grid = new Grid( tStart, tEnd, 2 );
269  nIntervals++;
270 
271  // ----------------------------------------------------
272  }
273 
274 
275  // CHECK WHETHER THE FUNCTION ITSELF IS VALID:
276  // -------------------------------------------
277  if( parameterization_.getNX () != 0 ||
278  parameterization_.getNXA() != 0 ||
279  parameterization_.getNP () != 0 ||
280  parameterization_.getNPI() != 0 ||
281  parameterization_.getNU () != 0 ||
282  parameterization_.getNUI() != 0 ||
283  parameterization_.getNW() != 0 ){
285  }
286 
287 
288  // SET THE DIMENSION OF THE CURVE:
289  // (the correctness of the dimension has already been cecked)
290  // ----------------------------------------------------------
291 
292  dim = parameterization_.getDim();
293 
294 
295  // ALLOCATE MEMORY FOR THE NEW PIECE OF CURVE:
296  // -------------------------------------------
297 
299 
300 
301  // MAKE A DEEP COPY OF THE PARAMETERIZATION:
302  // -----------------------------------------
303 
304  parameterization[nIntervals-1] = new Function(parameterization_);
305 
306 
307  // RETURN:
308  // -----------------------------------------
309 
310  return SUCCESSFUL_RETURN;
311 }
312 
313 
314 returnValue Curve::evaluate( double t, double *result ) const{
315 
316  uint idx ;
317  returnValue returnvalue;
318 
319  // CHECK WHETHER THE CURVE IS EMPTY:
320  // ---------------------------------
321  if( isEmpty() == BT_TRUE )
323 
324 
325  // CHECK WHETHER THE TIME t IS IN THE DOMAIN OF THE CURVE:
326  // ---------------------------------------------------------
327  if( (t > grid->getLastTime() + 100.0*EPS) || (t < grid->getFirstTime() - 100.0*EPS) )
329 
330 
331  // CHECK WHETHER THE ARGUMENT IS THE NULL POINTER:
332  // -----------------------------------------------
333  if( result == 0 )
335 
336 
337  // OBTAIN THE INTERVAL INDEX:
338  // --------------------------
339 
340  idx = grid->getFloorIndex(t);
341  if( idx == nIntervals ) idx--;
342 
343 
344  // EVALUATE THE FUNCTION ASSOCIATED WITH THIS INTERVAL:
345  // ----------------------------------------------------
346  double tt[1] = { t };
347  returnvalue = parameterization[idx]->evaluate(0,tt,result);
348 
349 
350  if( returnvalue != SUCCESSFUL_RETURN )
351  return ACADOERROR(returnvalue);
352 
353  return SUCCESSFUL_RETURN;
354 }
355 
356 
357 returnValue Curve::evaluate( double t, DVector &result ) const{
358 
359  uint run1 ;
360  returnValue returnvalue;
361 
362  double *tmp = new double[dim];
363 
364  returnvalue = evaluate( t, tmp );
365 
366  if( returnvalue != SUCCESSFUL_RETURN )
367  return returnvalue;
368 
369  result.init(dim);
370  for( run1 = 0; run1 < dim; run1++ )
371  result(run1) = tmp[run1];
372 
373  delete[] tmp;
374 
375  return SUCCESSFUL_RETURN;
376 }
377 
378 
379 returnValue Curve::evaluate( double tStart, double tEnd, VariablesGrid &result ) const
380 {
381  // determine sub grid of intervals with given time horizon [tStart,tEnd]
382  Grid intervalsSubGrid;
383 
384  if ( grid->getSubGrid( tStart,tEnd,intervalsSubGrid ) != SUCCESSFUL_RETURN )
385  return ACADOERROR( RET_UNKNOWN_BUG );
386 
387  return discretize( intervalsSubGrid,result );
388 }
389 
390 
391 
392 returnValue Curve::discretize( const Grid &discretizationGrid, VariablesGrid &result ) const{
393 
394  uint run1 ;
395  returnValue returnvalue;
396  DVector tmp ;
397 
398  result.init( dim, discretizationGrid );
399 
400  for( run1 = 0; run1 < discretizationGrid.getNumPoints(); run1++ ){
401  returnvalue = evaluate( discretizationGrid.getTime(run1), tmp );
402  if( returnvalue != SUCCESSFUL_RETURN )
403  return returnvalue;
404  result.setVector(run1,tmp);
405  }
406  return SUCCESSFUL_RETURN;
407 }
408 
409 
410 returnValue Curve::getTimeDomain( double tStart, double tEnd ) const
411 {
412  // CHECK WHETHER THE CURVE IS EMPTY:
413  // ---------------------------------
414  if( isEmpty() == BT_TRUE )
416 
417  tStart = grid->getFirstTime();
418  tEnd = grid->getLastTime() ;
419 
420  return SUCCESSFUL_RETURN;
421 }
422 
423 
424 returnValue Curve::getTimeDomain( const uint &idx, double tStart, double tEnd ) const
425 {
426  // CHECK WHETHER THE CURVE IS EMPTY:
427  // ---------------------------------
428  if( isEmpty() == BT_TRUE )
430 
431 
432  // CHECK WHETHER THE INDEX IS IN THE PERMISSIBLE RANGE:
433  // ----------------------------------------------------
434  if( idx >= grid->getNumIntervals() )
436 
437 
438  tStart = grid->getTime(idx );
439  tEnd = grid->getTime(idx+1);
440 
441  return SUCCESSFUL_RETURN;
442 }
443 
444 
446  ) const
447 {
448  if( isEmpty( ) == BT_TRUE )
449  return BT_FALSE;
450 
451  if ( ( acadoIsGreater( t,grid->getFirstTime() ) == BT_TRUE ) &&
452  ( acadoIsSmaller( t,grid->getLastTime() ) == BT_TRUE ) )
453  return BT_TRUE;
454  else
455  return BT_FALSE;
456 }
457 
458 
459 
460 //
461 // PROTECTED MEMBER FUNCTIONS:
462 //
463 
464 
466 
467 /*
468  * end of file
469  */
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
int getNPI() const
Definition: function.cpp:239
double getTime(uint pointIdx) const
double getFirstTime() const
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
returnValue evaluate(double t, double *result) const
Definition: curve.cpp:314
BEGIN_NAMESPACE_ACADO const double EPS
returnValue add(double tStart, double tEnd, const DVector constant)
Definition: curve.cpp:143
void init(unsigned _dim=0)
Definition: vector.hpp:155
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
DVector evaluate(const EvaluationPoint &x, const int &number=0)
Definition: function.cpp:520
int getNUI() const
Definition: function.cpp:227
BooleanType isInTimeDomain(double t) const
Definition: curve.cpp:445
int getNU() const
Definition: function.cpp:222
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
Curve()
Definition: curve.cpp:45
uint nIntervals
Definition: curve.hpp:301
BooleanType acadoIsGreater(double x, double y, double TOL)
#define CLOSE_NAMESPACE_ACADO
returnValue getSubGrid(double tStart, double tEnd, Grid &_subGrid) const
Definition: grid.cpp:655
int getNXA() const
Definition: function.cpp:212
uint getFloorIndex(double time) const
Definition: grid.cpp:593
uint dim
Definition: curve.hpp:302
Function ** parameterization
Definition: curve.hpp:303
Grid * grid
Definition: curve.hpp:304
int getNW() const
Definition: function.cpp:245
~Curve()
Definition: curve.cpp:72
int getNP() const
Definition: function.cpp:233
Curve & operator=(const Curve &arg)
Definition: curve.cpp:85
returnValue setVector(uint pointIdx, const DVector &_values)
unsigned getDim() const
Definition: vector.hpp:172
int getDim() const
returnValue init()
BooleanType acadoIsSmaller(double x, double y, double TOL)
Allows to work with piecewise-continous function defined over a scalar time interval.
Definition: curve.hpp:52
returnValue getTimeDomain(double tStart, double tEnd) const
Definition: curve.cpp:410
#define ASSERT(x)
#define BT_TRUE
Definition: acado_types.hpp:47
Curve operator()(uint idx) const
Definition: curve.cpp:114
uint getNumIntervals() const
uint getNumPoints() const
int getNX() const
Definition: function.cpp:207
double getLastTime() const
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
returnValue discretize(const Grid &discretizationGrid, VariablesGrid &result) const
Definition: curve.cpp:392
BooleanType acadoIsStrictlyGreater(double x, double y, double TOL)
int getDim() const
BooleanType isEmpty() const
#define ACADOERROR(retval)
InterpolationMode


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