/* >>> start tutorial code >>> */ int main( ){ USING_NAMESPACE_ACADO // DEFINE VALRIABLES: // --------------------------- DifferentialState x(2); DifferentialState y(2); Function f; // DEFINE A TEST FUNCTION: // ----------------------- IntermediateState ff; ff = sin(x(0)*x(1)); f << forwardDerivative( ff + x(0) , x(0) ); f << forwardDerivative( x(1) + ff , x(1) ); f << forwardDerivative( ff + ff , x ); f << forwardDerivative( ff + ff , x, y ); f << forwardDerivative( ff + ff , x, dot(x) ); //f << dot(sin(x(0)*x(1))); FILE *file = fopen("symbolic_differentiation3_output.txt", "w" ); file << f; fclose(file); return 0; }
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; }
/* >>> 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; }