C++ Expression::getDim方法代码示例

returnValue DiscreteTimeExport::setDifferentialEquation(	const Expression& rhs_ )
{
	if( rhs_.getDim() > 0 ) {
		Parameter         dummy0;
		Control           dummy1;
		DifferentialState dummy2;
		AlgebraicState 	  dummy3;
		DifferentialStateDerivative dummy4;
		dummy0.clearStaticCounters();
		dummy1.clearStaticCounters();
		dummy2.clearStaticCounters();
		dummy3.clearStaticCounters();
		dummy4.clearStaticCounters();

		NX2 = rhs_.getDim() - NXA;
		x = DifferentialState(NX1+NX2);
		z = AlgebraicState(NXA);
		dx = DifferentialStateDerivative(NDX);
		u = Control(NU);
		p = Parameter(NP);

		DifferentialEquation f;
		f << rhs_;

		DifferentialEquation g;
		for( uint i = 0; i < rhs_.getDim(); i++ ) {
			g << forwardDerivative( rhs_(i), x );
			g << forwardDerivative( rhs_(i), u );
			// There are not supposed to be algebraic states or differential state derivatives !
		}

		return (rhs.init( f,"acado_rhs",NX,NXA,NU ) & diffs_rhs.init( g,"acado_diffs",NX,NXA,NU ) );
	}
	return SUCCESSFUL_RETURN;
}

returnValue PlotWindow::addSubplot(	const Expression& _expressionX,
									const Expression& _expressionY,
									const char* const _title,
									const char* const _xLabel,
									const char* const _yLabel,
									PlotMode _plotMode,
									double _xRangeLowerLimit,
									double _xRangeUpperLimit,
									double _yRangeLowerLimit,
									double _yRangeUpperLimit
									)
{

    ASSERT( _expressionX.getDim() == _expressionY.getDim() );

    uint run1;

    for( run1 = 0; run1 < _expressionX.getDim(); run1++ ){

		PlotWindowSubplot* newSubplot = new PlotWindowSubplot(	_expressionX(run1),_expressionY(run1),
																_title,_xLabel,_yLabel,_plotMode,
																_xRangeLowerLimit,_xRangeUpperLimit,
																_yRangeLowerLimit,_yRangeUpperLimit );

		if ( number == 0 )
		{
			first = newSubplot;
			last = newSubplot;
		}
		else
		{
			if ( last->setNext( newSubplot ) != SUCCESSFUL_RETURN )
				return ACADOERROR( RET_OPTIONS_LIST_CORRUPTED );
			last = newSubplot;
		}

        Expression tmpX( _expressionX(run1) );

		if ( addPlotDataItem( &tmpX ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_LOG_RECORD_CORRUPTED );

        Expression tmpY( _expressionY(run1) );

		if ( addPlotDataItem( &tmpY ) != SUCCESSFUL_RETURN )
			return ACADOERROR( RET_LOG_RECORD_CORRUPTED );

		++number;
    }

	return SUCCESSFUL_RETURN;
}

returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
{
	ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
	constraint->add(_lb, _expr, _ub);

	return SUCCESSFUL_RETURN;
}

returnValue ModelData::setModel( const DifferentialEquation& _f )
{
	if( rhs_name.empty() && NX2 == 0 ) {
		differentialEquation = _f;
		Expression rhs;
		differentialEquation.getExpression( rhs );

		NXA = differentialEquation.getNXA();
		NX2 = rhs.getDim() - NXA;
		if( NDX == 0 ) NDX = _f.getNDX();
//		if( _f.getNDX() > 0 && _f.getNDX() != (int)NX2 ) { // TODO: this test returns an error for well-defined models when using a linear input subsystem!
//			std::cout << "nonlinear model of size " << NX2 << " depends on " << _f.getNDX() << " differential state derivatives!" << std::endl;
//			return ACADOERROR( RET_INVALID_OPTION );
//		}
		if( NU == 0 ) NU = _f.getNU();
		if( NP == 0 ) NP = _f.getNP();
		if( NOD == 0 ) NOD = _f.getNOD();

		export_rhs = BT_TRUE;
	}
	else {
		return ACADOERROR( RET_INVALID_OPTION );
	}
	return SUCCESSFUL_RETURN;
}

Expression CFunction::operator()( const Expression &arg ){

    uint run1,run2;

    CFunction thisFunction(*this);
    thisFunction.nn = arg.getDim();

    for( run1 = 0; run1 < maxAlloc; run1++ ){
        thisFunction.xStore   [run1] = new double[thisFunction.nn];
        thisFunction.seedStore[run1] = new double[thisFunction.nn];
        for( run2 = 0; run2 < thisFunction.nn; run2++ ){
            thisFunction.xStore   [run1][run2] = 0.0;
            thisFunction.seedStore[run1][run2] = 0.0;
        }
    }

    Expression tmp(dim);

    COperator dummy;
    dummy.increaseID();

    for( run1 = 0; run1 < dim; run1++ ){
        delete tmp.element[run1];
        tmp.element[run1] = new COperator( thisFunction, arg, run1 );
    }

    return tmp;
}

Expression KinVec::explicitize(const Expression & ddq_) const {

    if ( q.getDim() != 3 ){ ACADOERROR( RET_DDQ_DIMENSION_MISMATCH );           ASSERT( 1 == 0 ); }
    //if ( abs(order) !=2  ){ ACADOERROR( RET_CAN_ONLY_SOLVE_2ND_ORDER_KINVECS ); ASSERT( 1 == 0 ); }
    if ( !J.getDim()) throw("KinVec jacobian information was lost. Some operators destroy this information. (e.g. cross)");

    //Expression Jc;               // SHOULD BE INTERMEDIATE STATE.
    IntermediateState Jc((uint) 3,ddq_.getDim());       
    IntermediateState cc=c;

    uint i;
    
    std::map<uint,uint> di;
    
    uint j;
    uint count;
    Matrix p=ddq.getDependencyPattern(ddq_);

    for ( j = 0; j < ddq_.getDim(); j++ ){
       for ( i = 0; i < ddq.getDim(); i++ ){
	 if (p(i,j)>0.0) {
	    di[i]=count;
	    count++;
	 }
       }
    }
    
    for ( j = 0; j < ddq.getDim(); j++ ){
      for ( i = 0; i < 3; i++ ){
	if (di.count(j)) {
	  Jc(i,di[j])=J(i,j);
	} else {
	  cc(i)+=J(i,j)*ddq(j);
	}
      }
    }

    printf("test\n");
    Function ff;
    ff << Jc;
    FILE *myfile=fopen("debug.txt","w");
    myfile << ff;
    fclose(myfile);
    Jc.getInverse();
    return -Jc.getInverse()*cc;
}

Expression Frame::chain(Expression e_,const Frame &ei,bool type) const {
    if (e_.getDim()==1 or e_.getDim()==0) return e_; // empty expression
    //printf("Chain Expression %d x %d",e_.getNumRows(),e_.getNumCols());
    FrameNode* common=getCommonFramePtr(ei);

    std::vector<FrameNode*> thisChain=     getFrameChain(common);
    std::vector<FrameNode*> eiChain  =  ei.getFrameChain(common);

    Expression e(e_);
    for (std::vector<FrameNode*>::iterator it=thisChain.begin() ; it != thisChain.end(); ++it ) {

        e=(*it)->R * e; // Going up
        if (type) e=e+(*it)->p;
    }

    for (std::vector<FrameNode*>::reverse_iterator it=eiChain.rbegin() ; it != eiChain.rend(); ++it ) {

        e=((*it)->R).transpose()*e; // Going down
        if (type) e=e-((((*it)->R).transpose() * (*it)->p));
    }
    return e;
}

BooleanType FunctionEvaluationTree::isRationalIn( const Expression &variable ){

    int nn = variable.getDim();

    int                 run1;
    BooleanType *implicit_dep = new BooleanType [n ];
    VariableType *varType     = new VariableType[nn];
    int          *component   = new int         [nn];

    for( run1 = 0; run1 < nn; run1++ ){

        Operator *tmp2 = (variable.element[run1])->clone();

        if( tmp2->isVariable( varType[run1], component[run1] ) == BT_FALSE ){

             ACADOERROR(RET_INVALID_ARGUMENTS);
             delete   tmp2     ;
             delete[] varType  ;
             delete[] component;
             return   BT_TRUE  ;
        }

        if( varType[run1] == VT_INTERMEDIATE_STATE ){

             ACADOERROR(RET_INVALID_ARGUMENTS);
             delete   tmp2     ;
             delete[] varType  ;
             delete[] component;
             return   BT_TRUE  ;
        }
        delete tmp2;
    }


    for( run1 = 0; run1 < n; run1++ ){
        implicit_dep[run1] = sub[run1]->isRationalIn( 1, varType, component, implicit_dep );
    }
    for( run1 = 0; run1 < dim; run1++ ){
        if( f[run1]->isRationalIn( 1, varType, component, implicit_dep ) == BT_FALSE  ){
            delete[] implicit_dep;
            delete[] varType  ;
            delete[] component;
            return BT_FALSE;
        }
    }

    delete[] implicit_dep;
    delete[] varType  ;
    delete[] component;
    return   BT_TRUE;
}

uint ModelData::addOutput( const OutputFcn& outputEquation_ ){

	if( rhs_name.isEmpty() && outputNames.size() == 0 ) {
		Expression next;
		outputEquation_.getExpression( next );
		outputExpressions.push_back( next );
		dim_outputs.push_back( next.getDim() );
	}
	else {
		return ACADOERROR( RET_INVALID_OPTION );
	}

	return dim_outputs.size();
}

returnValue DiscreteTimeExport::setDifferentialEquation(	const Expression& rhs_ )
{
	if( rhs_.getDim() > 0 ) {
		OnlineData        dummy0;
		Control           dummy1;
		DifferentialState dummy2;
		AlgebraicState 	  dummy3;
		DifferentialStateDerivative dummy4;
		dummy0.clearStaticCounters();
		dummy1.clearStaticCounters();
		dummy2.clearStaticCounters();
		dummy3.clearStaticCounters();
		dummy4.clearStaticCounters();

		NX2 = rhs_.getDim() - NXA;
		x = DifferentialState("", NX1+NX2, 1);
		z = AlgebraicState("", NXA, 1);
		dx = DifferentialStateDerivative("", NDX, 1);
		u = Control("", NU, 1);
		od = OnlineData("", NOD, 1);

		DifferentialEquation f;
		f << rhs_;

		DifferentialEquation g;
		for( uint i = 0; i < rhs_.getDim(); i++ ) {
			g << forwardDerivative( rhs_(i), x );
			g << forwardDerivative( rhs_(i), u );
			// There are not supposed to be algebraic states or differential state derivatives !
		}

		return (rhs.init(f, "rhs", NX, NXA, NU, NP, NDX, NOD) &
				diffs_rhs.init(g, "diffs", NX, NXA, NU, NP, NDX, NOD));
	}
	return SUCCESSFUL_RETURN;
}

returnValue ModelData::setModel( const DifferentialEquation& _f )
{
	if( rhs_name.isEmpty() && NX2 == 0 ) {
		differentialEquation = _f;
		Expression rhs;
		differentialEquation.getExpression( rhs );

		NX2 = rhs.getDim() - differentialEquation.getNXA();
		if( NDX == 0 ) NDX = differentialEquation.getNDX();
		NXA = differentialEquation.getNXA();
		if( NU == 0 ) NU = differentialEquation.getNU();
		if( NP == 0 ) NP = differentialEquation.getNP();

		model_dimensions_set = BT_TRUE;
		export_rhs = BT_TRUE;
	}
	else {
		return ACADOERROR( RET_INVALID_OPTION );
	}
	return SUCCESSFUL_RETURN;
}

returnValue FunctionEvaluationTree::operator<<( const Expression& arg ){

    safeCopy << arg;

    uint run1;

    for( run1 = 0; run1 < arg.getDim(); run1++ ){

        int nn;

        nn = n;

        f = (Operator**)realloc(f,(dim+1)*sizeof(Operator*));

        f[dim] = arg.element[run1]->clone();
        f[dim]-> loadIndices  ( indexList );

        sub       = (Operator**)realloc(sub,
                    (indexList->getNumberOfOperators())*sizeof(Operator*));
        lhs_comp  = (int*)realloc(lhs_comp,
                    (indexList->getNumberOfOperators())*sizeof(int));

        indexList->getOperators( sub, lhs_comp, &n );

        while( nn < n ){

            sub[nn]-> enumerateVariables( indexList );
            nn++;
        }

        f[dim]-> enumerateVariables( indexList );

        dim++;
    }
    return SUCCESSFUL_RETURN;
}

uint ModelData::addOutput( const OutputFcn& outputEquation_, const Grid& grid ){

	if( rhs_name.empty() && outputNames.size() == 0 ) {
		Expression next;
		outputEquation_.getExpression( next );
		outputExpressions.push_back( next );
		dim_outputs.push_back( next.getDim() );

		if( NDX == 0 ) NDX = outputEquation_.getNDX();
		if( NU == 0 ) NU = outputEquation_.getNU();
		if( NP == 0 ) NP = outputEquation_.getNP();
		if( NOD == 0 ) NOD = outputEquation_.getNOD();

		outputGrids.push_back( grid );

		uint numOuts = (int) ceil((double)grid.getNumIntervals());
		num_meas.push_back( numOuts );
	}
	else {
		return ACADOERROR( RET_INVALID_OPTION );
	}

	return dim_outputs.size();
}

/* >>> start tutorial code >>> */
int main( ) {

    DifferentialState   xT;     // the trolley position
    DifferentialState   vT;     // the trolley velocity
    IntermediateState   aT;     // the trolley acceleration
    DifferentialState   xL;     // the cable length
    DifferentialState   vL;     // the cable velocity
    IntermediateState   aL;     // the cable acceleration
    DifferentialState   phi;    // the excitation angle
    DifferentialState   omega;  // the angular velocity

    DifferentialState   uT;     // trolley velocity control
    DifferentialState   uL;     // cable velocity control

    Control             duT;
    Control             duL;

    //
    // DEFINE THE PARAMETERS:
    //
    const double      tau1 = 0.012790605943772;
    const double      a1   = 0.047418203070092;
    const double      tau2 = 0.024695192379264;
    const double      a2   = 0.034087337273386;
    const double      g = 9.81;
    const double      c = 0.2;
    const double      m = 1318.0;

    //
    // DEFINE THE MODEL EQUATIONS:
    //
    DifferentialEquation   f, f2, test_expr;
    ExportAcadoFunction    fun, fun2;

    aT = -1.0 / tau1 * vT + a1 / tau1 * uT;
    aL = -1.0 / tau2 * vL + a2 / tau2 * uL;

    Expression states;
    states << xT;
    states << vT;
    states << xL;
    states << vL;
    states << phi;
    states << omega;
    states << uT;
    states << uL;

    Expression controls;
    controls << duT;
    controls << duL;

    Expression arg;
    arg << states;
    arg << controls;

    int NX = states.getDim();
    int NU = controls.getDim();

    IntermediateState expr(2);

    expr(0) = - 1.0/xL*(-g*sin(phi)-aT*cos(phi)-2*vL*omega-c*omega/(m*xL)) + log(duT/duL)*pow(xL,2);
    expr(1) = - 1.0/xL*(-g*tan(phi)-aT*acos(phi)-2*atan(vL)*omega-c*asin(omega)/exp(xL)) + duT/pow(omega,3);
    //~ expr(0) = - 1.0/xL*(-g*sin(phi));
    //~ expr(1) = duT/pow(omega,3);

    DifferentialState lambda("", expr.getDim(),1);
    DifferentialState Sx("", states.getDim(),states.getDim());
    DifferentialState Su("", states.getDim(),controls.getDim());
    Expression S = Sx;
    S.appendCols(Su);

    // SYMMETRIC DERIVATIVES
    Expression S_tmp = S;
    S_tmp.appendRows(zeros<double>(NU,NX).appendCols(eye<double>(NU)));

    IntermediateState dfS,dl;

    Expression f_tmp = symmetricDerivative( expr, arg, S_tmp, lambda, &dfS, &dl );
    f << returnLowerTriangular( f_tmp );

    fun.init(f, "symmetricDerivative", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0);

    // ALTERNATIVE DERIVATIVES
    IntermediateState tmp = backwardDerivative( expr, states, lambda );
    IntermediateState tmp2 = forwardDerivative( tmp, states );
    Expression tmp3 = backwardDerivative( expr, controls, lambda );
    Expression tmp4 = multipleForwardDerivative( tmp3, states, Su );
    Expression tmp5 = tmp4 + tmp4.transpose() + forwardDerivative( tmp3, controls );

    Expression f2_tmp1;
    f2_tmp1 = symmetricDoubleProduct( tmp2, Sx );
    f2_tmp1.appendCols( zeros<double>(NX,NU) );

    Expression f2_tmp2;
    f2_tmp2 = Su.transpose()*tmp2*Sx + multipleForwardDerivative( tmp3, states, Sx );
    f2_tmp2.appendCols( symmetricDoubleProduct( tmp2, Su ) + tmp5 );

    f2_tmp1.appendRows( f2_tmp2 );
    f2 << returnLowerTriangular( f2_tmp1 );
//.........这里部分代码省略.........