Ejemplo n.º 1
0
returnValue SCPmethod::shiftVariables(	double timeShift,
										Vector  lastX,
										Vector  lastXA,
										Vector  lastP,
										Vector  lastU,
										Vector  lastW
										)
{
	#ifdef SIM_DEBUG
	acadoPrintf( "SCPmethod::shiftVariables\n" );
	#endif
	
	if ( acadoIsNegative( timeShift ) == BT_TRUE )
		return ACADOERROR( RET_INVALID_ARGUMENTS );

// 	Matrix tmp;
// 	for( uint i=1; i<iter.getNumPoints()-1; ++i )
// 	{
// 		bandedCP.lambdaDynamic.getSubBlock( i,0, tmp );
// 		bandedCP.lambdaDynamic.setDense( i-1,0, tmp );
// 	}

// 	printf("shifted!\n");
	needToReevaluate = BT_TRUE;
	return iter.shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
}
Ejemplo n.º 2
0
returnValue ShootingMethod::evaluate(	OCPiterate &iter
										)
{
// 	iter.print(); ASSERT( 1==0 );

    // INTRODUCE SOME AUXILIARY VARIABLES:
    // -----------------------------------
    ASSERT( iter.x != 0 );

    uint run1;
    double tStart, tEnd;

    Vector x ;  nx = iter.getNX ();
    Vector xa;  na = iter.getNXA();
    Vector p ;  np = iter.getNP ();
    Vector u ;  nu = iter.getNU ();
    Vector w ;  nw = iter.getNW ();

	VariablesGrid xAll;
	VariablesGrid xaAll;

    residuum = *iter.x;
    residuum.setAll( 0.0 );

    iter.getInitialData( x, xa, p, u, w );
// 	iter.print();

    // RUN A LOOP OVER ALL INTERVALS OF THE UNION GRID:
    // ------------------------------------------------

    for( run1 = 0; run1 < unionGrid.getNumIntervals(); run1++ ){

        integrator[run1]->setOptions( getOptions( 0 ) );  // ??

		int freezeIntegrator;
		get( FREEZE_INTEGRATOR, freezeIntegrator );

		if ( (BooleanType)freezeIntegrator == BT_TRUE )
			integrator[run1]->freezeAll();

        tStart = unionGrid.getTime( run1   );
        tEnd   = unionGrid.getTime( run1+1 );
		
// 		printf("tStart = %e,   tEnd = %e\n",tStart,tEnd );
// 		x.print("x");
// 		u.print("u");
// 		p.print("p");

    //  integrator[run1]->set( INTEGRATOR_PRINTLEVEL, MEDIUM );

		Grid evaluationGrid;
		iter.x->getSubGrid( tStart,tEnd,evaluationGrid );
		
        Grid outputGrid;
		if ( acadoIsNegative( integrator[run1]->getDifferentialEquationSampleTime( ) ) == BT_TRUE )
			outputGrid.init( tStart,tEnd,getNumEvaluationPoints() );
		else
			outputGrid.init( tStart,tEnd, 1+acadoRound( (tEnd-tStart)/integrator[run1]->getDifferentialEquationSampleTime() ) );

        if ( integrator[run1]->integrate( outputGrid&evaluationGrid, x, xa, p, u, w ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_UNABLE_TO_INTEGRATE_SYSTEM );

		Vector xOld;
		Vector pOld = p;
		
		if ( evaluationGrid.getNumPoints( ) <= 2 )
		{
			integrator[run1]->getX ( x  );
			integrator[run1]->getXA( xa );

			xOld = x;
			
			iter.updateData( tEnd, x, xa, p, u, w );
		}
		else
		{
			integrator[run1]->getX (  xAll );
			integrator[run1]->getXA( xaAll );

			xOld = xAll.getLastVector( );
			
			for( uint run2=1; run2<outputGrid.getNumPoints(); ++run2 )
			{
				if ( evaluationGrid.hasTime( outputGrid.getTime(run2) ) == BT_TRUE )
				{
					x  =  xAll.getVector(run2);
					xa = xaAll.getVector(run2);
					iter.updateData( outputGrid.getTime(run2), x, xa, p, u, w );
				}
			}
		}

		if ( iter.isInSimulationMode( ) == BT_FALSE )
			p = pOld;  // should be changed later...

        residuum.setVector( run1, xOld - x );
    }

    // LOG THE RESULTS:
    // ----------------
    return logTrajectory( iter );
}