returnValue OCP::subjectTo( const int index_, const ConstraintComponent& component ){
for( uint i=0; i<component.getDim(); ++i )
constraint.add( index_,component(i) );
return SUCCESSFUL_RETURN;
}
returnValue OCP::subjectTo( const TimeHorizonElement index_, const ConstraintComponent& component ){
uint i;
switch( index_ ){
case AT_START:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( 0,component(i) ) );
return SUCCESSFUL_RETURN;
case AT_END:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( grid.getLastIndex(),component(i) ) );
return SUCCESSFUL_RETURN;
default:
return ACADOERROR(RET_UNKNOWN_BUG);
}
return SUCCESSFUL_RETURN;
}
returnValue OCP::subjectTo( int index_, const ConstraintComponent& component )
{
ASSERT(index_ >= AT_START);
if (index_ == AT_START)
{
for (unsigned el = 0; el < component.getDim(); ++el)
ACADO_TRY( constraint->add( 0,component( el ) ) );
}
else if (index_ == AT_END)
{
for (unsigned el = 0; el < component.getDim(); ++el)
ACADO_TRY(constraint->add(grid->getLastIndex(), component( el )));
}
else
{
for (unsigned el = 0; el < component.getDim(); ++el)
constraint->add(index_, component(el));
}
return SUCCESSFUL_RETURN;
}
returnValue Constraint::add( const int index_, const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
ASSERT_RETURN( index_ < (int) grid.getNumPoints() ).addMessage("\n >>> The constraint component can not be set as the associated discretization point is not in the time horizon. <<< \n\n");
uint run1;
if( component.hasLBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getLB()).getDim() == 1 )
tmp_lb(run1) = (component.getLB()).operator()(0);
else {
if( (component.getLB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_lb(run1) = (component.getLB()).operator()(run1);
}
}
}
else {
VariablesGrid LBgrid = component.getLBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
tmp_lb(run1) = tmp(0);
}
}
if( component.hasUBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getUB()).getDim() == 1 )
tmp_ub(run1) = (component.getUB()).operator()(0);
else {
if( (component.getUB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_ub(run1) = (component.getUB()).operator()(run1);
}
}
}
else {
VariablesGrid UBgrid = component.getUBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
tmp_ub(run1) = tmp(0);
}
}
ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );
return SUCCESSFUL_RETURN;
}
returnValue Constraint::add( const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
uint run1;
if( component.hasLBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getLB()).getDim() == 1 )
tmp_lb(run1) = (component.getLB()).operator()(0);
else {
if( (component.getLB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_lb(run1) = (component.getLB()).operator()(run1);
}
}
}
else {
VariablesGrid LBgrid = component.getLBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
tmp_lb(run1) = tmp(0);
}
}
if( component.hasUBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getUB()).getDim() == 1 )
tmp_ub(run1) = (component.getUB()).operator()(0);
else {
if( (component.getUB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_ub(run1) = (component.getUB()).operator()(run1);
}
}
}
else {
VariablesGrid UBgrid = component.getUBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
tmp_ub(run1) = tmp(0);
}
}
return add( tmp_lb, component.getExpression(), tmp_ub );
}
