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;
}
