80 index = (
int*)calloc(
nb,
sizeof(
int));
81 blb = (
double*)calloc(
nb,
sizeof(
double));
82 bub = (
double*)calloc(
nb,
sizeof(
double));
83 for( run1 = 0; run1 <
nb; run1++ ){
84 var [run1] = rhs.
var [run1];
86 blb [run1] = rhs.
blb [run1];
87 bub [run1] = rhs.
bub [run1];
101 if(
var != 0 ) free(
var);
103 if(
blb != 0 ) free(
blb);
104 if(
bub != 0 ) free(
bub);
113 if(
var != 0 ) free(
var);
115 if(
blb != 0 ) free(
blb);
116 if(
bub != 0 ) free(
bub);
125 index = (
int*)calloc(
nb,
sizeof(
int));
126 blb = (
double*)calloc(
nb,
sizeof(
double));
127 bub = (
double*)calloc(
nb,
sizeof(
double));
128 for( run1 = 0; run1 <
nb; run1++ ){
129 var [run1] = rhs.
var [run1];
131 blb [run1] = rhs.
blb [run1];
132 bub [run1] = rhs.
bub [run1];
165 blb = (
double* )realloc(
blb ,
nb*
sizeof(
double ));
166 bub = (
double* )realloc(
bub ,
nb*
sizeof(
double ));
168 var [
nb-1] = varType ;
180 lb[0] = (
double*)realloc(
lb[0],
fcn[0].getDim()*
sizeof(double));
181 ub[0] = (
double*)realloc(
ub[0],
fcn[0].getDim()*
sizeof(double));
184 lb[0][fcn[0].getDim()-1] = lb_;
207 for( run1 = 0; run1 < nc; run1++ ){
208 resL( run1, 0 ) =
lb[0][run1] - result(run1);
209 resU( run1, 0 ) =
ub[0][run1] - result(run1);
260 for( run1 = 0; run1 < nBDirs; run1++ )
264 if(
nx > 0 ) Dx .
setRow( run1,
JJ[0].getX () );
265 if(
na > 0 ) Dxa.
setRow( run1,
JJ[0].getXA() );
266 if(
np > 0 ) Dp .
setRow( run1,
JJ[0].getP () );
267 if(
nu > 0 ) Du .
setRow( run1,
JJ[0].getU () );
268 if(
nw > 0 ) Dw .
setRow( run1,
JJ[0].getW () );
355 double *bseed1 =
new double[nc];
356 double *bseed2 =
new double[nc];
357 double *
R =
new double[nc];
359 double *
H =
new double[fcn[0].getNumberOfVariables() +1];
360 double *fseed =
new double[fcn[0].getNumberOfVariables() +1];
362 for( run1 = 0; run1 < nc; run1++ ){
363 bseed1[run1] = seed(run1,0);
367 for( run1 = 0; run1 < fcn[0].getNumberOfVariables()+1; run1++ )
384 for( run2 = 0; run2 <
nx; run2++ ){
389 fcn[0].AD_forward( 0, fseed, R );
390 for( run1 = 0; run1 < nc; run1++ )
391 Dx( run1, run2 ) = R[run1];
392 fseed[y_index[0][run2]] = 0.0;
396 for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
401 fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
403 for( run1 = 0 ; run1 <
nx ; run1++ ) Hx ( run2, run1 ) = -H[y_index[0][run1]];
404 for( run1 = nx ; run1 < nx+
na ; run1++ ) Hxa( run2, run1-nx ) = -H[y_index[0][run1]];
405 for( run1 = nx+na ; run1 < nx+na+
np ; run1++ ) Hp ( run2, run1-nx-na ) = -H[y_index[0][run1]];
406 for( run1 = nx+na+np ; run1 < nx+na+np+
nu ; run1++ ) Hu ( run2, run1-nx-na-np ) = -H[y_index[0][run1]];
407 for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+
nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
427 for( run2 = nx; run2 < nx+
na; run2++ ){
432 fcn[0].AD_forward( 0, fseed, R );
433 for( run1 = 0; run1 < nc; run1++ )
434 Dxa( run1, run2-nx ) = R[run1];
435 fseed[y_index[0][run2]] = 0.0;
439 for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
444 fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
446 for( run1 = 0 ; run1 <
nx ; run1++ ) Hx ( run2-nx, run1 ) = -H[y_index[0][run1]];
447 for( run1 = nx ; run1 < nx+
na ; run1++ ) Hxa( run2-nx, run1-nx ) = -H[y_index[0][run1]];
448 for( run1 = nx+na ; run1 < nx+na+
np ; run1++ ) Hp ( run2-nx, run1-nx-na ) = -H[y_index[0][run1]];
449 for( run1 = nx+na+np ; run1 < nx+na+np+
nu ; run1++ ) Hu ( run2-nx, run1-nx-na-np ) = -H[y_index[0][run1]];
450 for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+
nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
470 for( run2 = nx+na; run2 < nx+na+
np; run2++ ){
475 fcn[0].AD_forward( 0, fseed, R );
476 for( run1 = 0; run1 < nc; run1++ )
477 Dp( run1, run2-nx-na ) = R[run1];
478 fseed[y_index[0][run2]] = 0.0;
482 for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
487 fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
489 for( run1 = 0 ; run1 <
nx ; run1++ ) Hx ( run2-nx-na, run1 ) = -H[y_index[0][run1]];
490 for( run1 = nx ; run1 < nx+
na ; run1++ ) Hxa( run2-nx-na, run1-nx ) = -H[y_index[0][run1]];
491 for( run1 = nx+na ; run1 < nx+na+
np ; run1++ ) Hp ( run2-nx-na, run1-nx-na ) = -H[y_index[0][run1]];
492 for( run1 = nx+na+np ; run1 < nx+na+np+
nu ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np ) = -H[y_index[0][run1]];
493 for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+
nw; run1++ ) Hw ( run2-nx-na, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
514 for( run2 = nx+na+np; run2 < nx+na+np+
nu; run2++ ){
519 fcn[0].AD_forward( 0, fseed, R );
520 for( run1 = 0; run1 < nc; run1++ )
521 Du( run1, run2-nx-na-np ) = R[run1];
522 fseed[y_index[0][run2]] = 0.0;
526 for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
531 fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
533 for( run1 = 0 ; run1 <
nx ; run1++ ) Hx ( run2-nx-na-np, run1 ) = -H[y_index[0][run1]];
534 for( run1 = nx ; run1 < nx+
na ; run1++ ) Hxa( run2-nx-na-np, run1-nx ) = -H[y_index[0][run1]];
535 for( run1 = nx+na ; run1 < nx+na+
np ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na ) = -H[y_index[0][run1]];
536 for( run1 = nx+na+np ; run1 < nx+na+np+
nu ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np ) = -H[y_index[0][run1]];
537 for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+
nw; run1++ ) Hw ( run2-nx-na-np, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
557 for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+
nw; run2++ ){
562 fcn[0].AD_forward( 0, fseed, R );
563 for( run1 = 0; run1 < nc; run1++ )
564 Dw( run1, run2-nx-na-np-nu ) = R[run1];
565 fseed[y_index[0][run2]] = 0.0;
569 for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
574 fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
576 for( run1 = 0 ; run1 <
nx ; run1++ ) Hx ( run2-nx-na-np-nu, run1 ) = -H[y_index[0][run1]];
577 for( run1 = nx ; run1 < nx+
na ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx ) = -H[y_index[0][run1]];
578 for( run1 = nx+na ; run1 < nx+na+
np ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na ) = -H[y_index[0][run1]];
579 for( run1 = nx+na+np ; run1 < nx+na+np+
nu ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np ) = -H[y_index[0][run1]];
580 for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+
nw; run1++ ) Hw ( run2-nx-na-np-nu, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
612 for( run1 = 0; (int) run1 <
nb; run1++ ){
617 if( iter.
x != NULL ){
627 if( iter.
xa != NULL ){
636 if( iter.
p != NULL ){
645 if( iter.
u != NULL ){
654 if( iter.
w != NULL ){
686 double* dresult1 =
new double[nc ];
691 for( run1 = 0; run1 < (int) seed->
getNumCols(); run1++ ){
693 for( run2 = 0; run2 <= fcn[0].getNumberOfVariables(); run2++ )
696 for( run2 = 0; run2 < (int) seed->
getNumRows(); run2++ )
697 fseed1[
y_index[0][offset+run2]] = seed->operator()(run2,run1);
699 returnvalue = fcn[0].AD_forward( 0, fseed1, dresult1 );
705 for( run2 = 0; run2 < nc; run2++ )
706 tmp( run2, run1 ) = dresult1[run2];
BooleanType isVariable() const
returnValue add(const double lb_, const Expression &arg, const double ub_)
Data class for storing generic optimization variables.
Implements a very rudimentary block sparse matrix class.
ConstraintElement & operator=(const ConstraintElement &rhs)
void init(unsigned _nRows=0, unsigned _nCols=0)
double getTime(uint pointIdx) const
returnValue evaluate(const OCPiterate &iter)
virtual ~PointConstraint()
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Allows to pass back messages to the calling function.
DVector evaluate(const EvaluationPoint &x, const int &number=0)
returnValue evaluateSensitivities()
returnValue setZ(const uint &idx, const OCPiterate &iter)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Stores and evaluates pointwise constraints within optimal control problems.
returnValue setLowerBound(uint pointIdx, uint valueIdx, double _lb)
#define CLOSE_NAMESPACE_ACADO
uint getFloorIndex(double time) const
Base class for all variables within the symbolic expressions family.
returnValue computeForwardSensitivityBlock(int offset, int offset2, DMatrix *seed)
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
Base class for all kind of constraints (except for bounds) within optimal control problems...
returnValue init(uint _nRows, uint _nCols)
returnValue setUpperBound(uint pointIdx, uint valueIdx, double _ub)
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
void rhs(const real_t *x, real_t *f)
unsigned getNumRows() const
PointConstraint & operator=(const PointConstraint &rhs)
unsigned getNumCols() const
VariableType getVariableType() const
uint getNumPoints() const
uint getComponent(const unsigned int idx) const
#define BEGIN_NAMESPACE_ACADO
returnValue getBounds(const OCPiterate &iter)
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
int getNumberOfVariables() const
GenericVector< T > getRow(unsigned _idx) const
#define ACADOERROR(retval)