89 for( run1 = 0; run1 <
N; run1++ ){
106 const Grid &stageIntervals,
113 int integratorTypeTmp = integratorType_;
152 stageIndices(0,0) = stageIntervals.getNumIntervals() + tmp;
155 stageIndices(0,3) = differentialEquation_.
getStartTime();
156 stageIndices(0,4) = differentialEquation_.
getEndTime();
243 int freezeIntegrator;
266 outputGrid.
init( tStart,tEnd, 1+
acadoRound( (tEnd-tStart)/
integrator[run1]->getDifferentialEquationSampleTime() ) );
285 if ( evaluationGrid.getNumPoints( ) <= 2 )
304 if ( evaluationGrid.hasTime( outputGrid.
getTime(run2) ) ==
BT_TRUE )
341 for( run1 = 0; run1 < seed.
getNumRows(); run1++ ){
381 for( run1 = 0; run1 < n; run1++ ){
428 for( run1 = 0; run1 < n; run1++ ){
454 ddX.
setRow( run1, tmpX2 );
455 ddP.
setRow( run1, tmpP2 );
456 ddU.
setRow( run1, tmpU2 );
457 ddW.
setRow( run1, tmpW2 );
478 for( i = 0; i <
N; i++ ){
499 for( i = 0; i <
N; i++ ){
526 DMatrix S = ((A.transpose().eval() *
A)+E).inverse();
527 G += (B-G*
A)*(S*A.transpose());
538 DMatrix Gx, *Gu, b, d, D,
E, X, P,
U, W,
A, B;
541 for( i = 0; i <
N; i++ ){
559 for( i = 0; i <
N; i++ ){
562 for( j = 0; j < i; j++ ){
576 Gx = eye<double>(
nx);
595 for( i = 0; i <
N; i++ ){
597 DMatrix X, P,
U, W, D,
E, HX, HP, HU, HW,
S;
608 ACADO_TRY(
differentiateForwardBackward( i, X, E, E, E, S, D, HX, HP, HU, HW ));
612 if(
np > 0 ) hessian.
addDense( i, 2*NN+i, HP );
613 if(
nu > 0 ) hessian.
addDense( i, 3*NN+i, HU );
614 if(
nw > 0 ) hessian.
addDense( i, 4*NN+i, HW );
619 ACADO_TRY(
differentiateForwardBackward( i, E, P, E, E, S, D, HX, HP, HU, HW ));
622 if(
nx > 0 ) hessian.
addDense( 2*NN+i, i, HX );
623 if(
np > 0 ) hessian.
addDense( 2*NN+i, 2*NN+i, HP );
624 if(
nu > 0 ) hessian.
addDense( 2*NN+i, 3*NN+i, HU );
625 if(
nw > 0 ) hessian.
addDense( 2*NN+i, 4*NN+i, HW );
630 ACADO_TRY(
differentiateForwardBackward( i, E, E, U, E, S, D, HX, HP, HU, HW ));
633 if(
nx > 0 ) hessian.
addDense( 3*NN+i, i, HX );
634 if(
np > 0 ) hessian.
addDense( 3*NN+i, 2*NN+i, HP );
635 if(
nu > 0 ) hessian.
addDense( 3*NN+i, 3*NN+i, HU );
636 if(
nw > 0 ) hessian.
addDense( 3*NN+i, 4*NN+i, HW );
641 ACADO_TRY(
differentiateForwardBackward( i, E, E, E, W, S, D, HX, HP, HU, HW ));
644 if(
nx > 0 ) hessian.
addDense( 4*NN+i, i, HX );
645 if(
np > 0 ) hessian.
addDense( 4*NN+i, 2*NN+i, HP );
646 if(
nu > 0 ) hessian.
addDense( 4*NN+i, 3*NN+i, HU );
647 if(
nw > 0 ) hessian.
addDense( 4*NN+i, 4*NN+i, HW );
678 for(
int run1 = 0; run1 <
N; ++run1 )
690 for( run1 = 0; run1 <
N; run1++ )
710 double t1 = 0.0, t2 = 0.0;
717 intervalPoints(0,0) = 0.0;
720 for( i = 0; i <
N; i++ ){
727 if( i1 >= 0 ) t1 = iter.
p->operator()(0,i1);
730 if( i2 >= 0 ) t2 = iter.
p->operator()(0,i2);
737 intervalPoints(i+1,0) = intervalPoints(i,0) + tmp.
getNumPoints();
739 if ( ( i1 >= 0 ) || ( i2 >= 0 ) )
800 double newIntervalLength
VariablesGrid & shiftTimes(double timeShift)
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
GenericVector< T > getCol(unsigned _idx) const
returnValue setOptions(const Options &arg)
virtual returnValue getForwardSensitivities(BlockMatrix &D) const
Implements the Runge-Kutta-45 scheme for integrating ODEs.
Data class for storing generic optimization variables.
returnValue getX(DVector &xEnd) const
Implements a very rudimentary block sparse matrix class.
virtual returnValue clear()
virtual returnValue evaluate(OCPiterate &iter)
double getStartTime() const
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
virtual returnValue evaluateSensitivitiesLifted()
void init(unsigned _nRows=0, unsigned _nCols=0)
returnValue allocateIntegrator(uint idx, IntegratorType type_)
double getTime(uint pointIdx) const
int acadoMax(const int x, const int y)
double getFirstTime() const
virtual BooleanType isDiscretized() const
Implements the backward-differentiation formula for integrating DAEs.
GenericMatrix & appendCols(const GenericMatrix &_arg)
Stores a DifferentialEquation together with an OutputFcn.
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
BooleanType acadoIsNegative(double x, double TOL)
virtual returnValue deleteAllSeeds()
GenericMatrix & setCol(unsigned _idx, const GenericVector< T > &_arg)
returnValue rescale(VariablesGrid *trajectory, double tEndNew, double newIntervalLength) const
Discretizes a DifferentialEquation by means of single or multiple shooting.
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
virtual returnValue evaluateSensitivities()
virtual returnValue setBackwardSeed(const BlockMatrix &seed)
void copy(const ShootingMethod &arg)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
returnValue updateData(double t, DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector)
Allows to conveniently handle (one-dimensional) grids consisting of time points.
returnValue differentiateForwardBackward(const int &idx, const DMatrix &dX, const DMatrix &dP, const DMatrix &dU, const DMatrix &dW, const DMatrix &seed, DMatrix &D, DMatrix &ddX, DMatrix &ddP, DMatrix &ddU, DMatrix &ddW)
DynamicDiscretization & operator=(const DynamicDiscretization &rhs)
const DifferentialEquation & getDifferentialEquation(uint stageIdx=0) const
GenericMatrix & appendRows(const GenericMatrix &_arg)
virtual returnValue deleteAllSeeds()
#define CLOSE_NAMESPACE_ACADO
returnValue getSubGrid(double tStart, double tEnd, Grid &_subGrid) const
Base class for discretizing a DifferentialEquation for use in optimal control algorithms.
returnValue addOptionsList()
int getStartTimeIdx() const
returnValue setAll(double _value)
Implements the Runge-Kutta-23 scheme for integrating ODEs.
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
returnValue differentiateBackward(const int &idx, const DMatrix &seed, DMatrix &Gx, DMatrix &Gp, DMatrix &Gu, DMatrix &Gw)
ShootingMethod & operator=(const ShootingMethod &rhs)
void initializeVariables()
returnValue logTrajectory(const OCPiterate &iter)
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
returnValue setVector(uint pointIdx, const DVector &_values)
returnValue getInitialData(DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector) const
virtual DynamicDiscretization * clone() const
returnValue getI(VariablesGrid &I) const
returnValue init(uint _nRows, uint _nCols)
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
Implements the Runge-Kutta-78 scheme for integrating ODEs.
returnValue setAllVectors(const DVector &_values)
DVector getLastVector() const
virtual returnValue addTransition(const Transition &transition_)
returnValue setTransition(const Transition &trs)
returnValue init(uint _nPoints=0, const double *const _times=0)
Derived & setZero(Index size)
Encapsulates all user interaction for setting options, logging data and plotting results.
Implements the Runge-Kutta-12 scheme for integrating ODEs.
returnValue appendTimes(const VariablesGrid &arg, MergeMethod _mergeMethod=MM_DUPLICATE)
double getEndTime() const
virtual BooleanType isAffine() const
returnValue scaleTimes(double scaling)
DMatrix getMatrix(uint pointIdx) const
unsigned getNumRows() const
Implements the Runge-Kutta-45 scheme for integrating ODEs.
returnValue getXA(DVector &xaEnd) const
virtual returnValue unfreeze()
unsigned getNumCols() const
uint getNumIntervals() const
returnValue differentiateForward(const int &idx, const DMatrix &dX, const DMatrix &dP, const DMatrix &dU, const DMatrix &dW, DMatrix &D)
Allows to setup and evaluate transition functions based on SymbolicExpressions.
uint getNumPoints() const
Options getOptions(uint idx) const
DVector getVector(uint pointIdx) const
virtual returnValue setForwardSeed(const BlockMatrix &xSeed_, const BlockMatrix &pSeed_, const BlockMatrix &uSeed_, const BlockMatrix &wSeed_)
virtual returnValue init(const DifferentialEquation &rhs)=0
double getLastTime() const
#define BEGIN_NAMESPACE_ACADO
virtual ~ShootingMethod()
virtual returnValue getBackwardSensitivities(BlockMatrix &D) const
returnValue update(DMatrix &G, const DMatrix &A, const DMatrix &B)
virtual returnValue addStage(const DynamicSystem &dynamicSystem_, const Grid &stageIntervals, const IntegratorType &integratorType_=INT_UNKNOWN)
uint getNumEvaluationPoints() const
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
int getEndTimeIdx() const
BooleanType isInSimulationMode() const
GenericVector< T > getRow(unsigned _idx) const
Implements a scheme for evaluating discretized ODEs.
#define ACADOERROR(retval)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.