returnValue ThreeSweepsERKExport::setDifferentialEquation( const Expression& rhs_ ) { int sensGen; get( DYNAMIC_SENSITIVITY,sensGen ); OnlineData dummy0; Control dummy1; DifferentialState dummy2; AlgebraicState dummy3; DifferentialStateDerivative dummy4; dummy0.clearStaticCounters(); dummy1.clearStaticCounters(); dummy2.clearStaticCounters(); dummy3.clearStaticCounters(); dummy4.clearStaticCounters(); x = DifferentialState("", NX, 1); dx = DifferentialStateDerivative("", NDX, 1); z = AlgebraicState("", NXA, 1); u = Control("", NU, 1); od = OnlineData("", NOD, 1); if( NDX > 0 && NDX != NX ) { return ACADOERROR( RET_INVALID_OPTION ); } if( rhs_.getNumRows() != (NX+NXA) ) { return ACADOERROR( RET_INVALID_OPTION ); } DifferentialEquation f, g, h, f_ODE; // add usual ODE f_ODE << rhs_; if( f_ODE.getNDX() > 0 ) { return ACADOERROR( RET_INVALID_OPTION ); } uint numX = NX*(NX+1)/2.0; uint numU = NU*(NU+1)/2.0; uint numZ = (NX+NU)*(NX+NU+1)/2.0; if( (ExportSensitivityType)sensGen == SYMMETRIC ) { // SWEEP 1: // --------- f << rhs_; // SWEEP 2: // --------- DifferentialState lx("", NX,1); Expression tmp = backwardDerivative(rhs_, x, lx); g << tmp; // SWEEP 3: // --------- DifferentialState Gx("", NX,NX), Gu("", NX,NU); DifferentialState H("", numZ,1); Expression S = Gx; S.appendCols(Gu); Expression arg; arg << x; arg << u; // SYMMETRIC DERIVATIVES Expression S_tmp = S; S_tmp.appendRows(zeros<double>(NU,NX).appendCols(eye<double>(NU))); Expression dfS; Expression h_tmp = symmetricDerivative( rhs_, arg, S_tmp, lx, &dfS ); h << dfS.getCols(0,NX); h << dfS.getCols(NX,NX+NU); h << returnLowerTriangular( h_tmp ); // OLD VERSION: // // add VDE for differential states // h << multipleForwardDerivative( rhs_, x, Gx ); // // // add VDE for control inputs // h << multipleForwardDerivative( rhs_, x, Gu ) + forwardDerivative( rhs_, u ); // // IntermediateState tmp2 = forwardDerivative(tmp, x); // Expression tmp3 = backwardDerivative(rhs_, u, lx); // Expression tmp4 = multipleForwardDerivative(tmp3, x, Gu); // // // TODO: include a symmetric_AD_operator to strongly improve the symmetric left-right multiplied second order derivative computations !! //// Expression tmp6 = Gx.transpose()*tmp2*Gx; // h << symmetricDoubleProduct(tmp2, Gx); // h << Gu.transpose()*tmp2*Gx + multipleForwardDerivative(tmp3, x, Gx); // Expression tmp7 = tmp4 + tmp4.transpose() + forwardDerivative(tmp3, u); // h << symmetricDoubleProduct(tmp2, Gu) + returnLowerTriangular(tmp7); } else { return ACADOERROR( RET_INVALID_OPTION ); } if( f.getNT() > 0 ) timeDependant = true; return rhs.init(f, "acado_forward", NX, 0, NU, NP, NDX, NOD) & diffs_rhs.init(g, "acado_backward", 2*NX, 0, NU, NP, NDX, NOD) & diffs_sweep3.init(h, "acado_forward_sweep3", 2*NX + NX*(NX+NU) + numX + NX*NU + numU, 0, NU, NP, NDX, NOD); }
/* >>> 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; }