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 OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub ) { ASSERT(_index >= AT_START); cout << _lb.getDim() << " " << _expr.getDim() << endl; ASSERT(_lb.getDim() == _expr.getDim()); ASSERT(_lb.getDim() == _ub.getDim()); if (_index == AT_START) { for (unsigned el = 0; el < _lb.getDim(); ++el) ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) ); } else if (_index == AT_END) { for (unsigned el = 0; el < _lb.getDim(); ++el) ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) ); } else for (unsigned el = 0; el < _lb.getDim(); ++el) constraint->add(_index, _lb( el ), _expr( el ), _ub( el )); return SUCCESSFUL_RETURN; }
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 ); fun2.init(f2, "alternativeSymmetric", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0); Function f1; f1 << dfS; f1 << dl; std::ofstream stream2( "ADtest/ADsymbolic_output2.c" ); stream2 << f1; stream2.close(); std::ofstream stream( "ADtest/ADsymbolic_output.c" ); fun.exportCode( stream, "double" ); fun2.exportCode( stream, "double" ); test_expr << expr; stream << test_expr; stream.close(); return 0; }