SX MultipleShooting::getOutput(string o)
{
SX ret = ssym(o, N);
for (int k=0; k<N; k++)
ret.at(k) = getOutput(o, k);
return ret;
}
void
dxdt(map<string,SX> &xDot, map<string,SX> &outputs, map<string,SX> state, map<string,SX> action, map<string,SX> param, SX t)
{
double g = 9.8;
double L1 = 0.1;
double L2 = 0.1;
double m0 = 0.1;
double mp = 0.03;
double m1 = mp;
double m2 = mp;
double d1 = m0 + m1 + m2;
double d2 = (0.5*m1 + m2)*L1;
double d3 = 0.5 * m2 * L2;
double d4 = ( m1/3 + m2 )*SQR(L1);
double d5 = 0.5 * m2 * L1 * L2;
double d6 = m2 * SQR(L2)/3;
double f1 = (0.5*m1 + m2) * L1 * g;
double f2 = 0.5 * m2 * L2 * g;
// th0 = y(1);
// th1 = y(2);
// th2 = y(3);
// th0d = y(4);
// th1d = y(5);
// th2d = y(6);
SX th0 = state["th0"];
SX th1 = state["th1"];
SX th2 = state["th2"];
SX th0d = state["th0d"];
SX th1d = state["th1d"];
SX th2d = state["th2d"];
// D = [ d1, d2*cos(th1), d3*cos(th2);
// d2*cos(th1), d4, d5*cos(th1-th2);
// d3*cos(th2), d5*cos(th1-th2), d6;];
SX D( zerosSX(3,3) );
makeDense(D);
D[0,0] = d1; D[0,1] = d2*cos(th1); D[0,2] = d3*cos(th2);
D[1,0] = d2*cos(th1); D[1,1] = d4; D[1,2] = d5*cos(th1-th2);
D[2,0] = d3*cos(th2); D[2,1] = d5*cos(th1-th2); D[2,2] = d6;
// C = [0, -d2*sin(th1)*th1d, -d3*sin(th2)*th2d;
// 0, 0, d5*sin(th1-th2)*th2d;
// 0, -d5*sin(th1-th2)*th1d, 0;];
SX C( zerosSX(3,3) );
makeDense(C);
C[0,0] = 0; C[0,1] = -d2*sin(th1)*th1d; C[0,2] = -d3*sin(th2)*th2d;
C[1,0] = 0; C[1,1] = 0; C[1,2] = d5*sin(th1-th2)*th2d;
C[2,0] = 0; C[2,1] = -d5*sin(th1-th2)*th1d; C[2,2] = 0;
// G = [0; -f1*sin(th1); -f2*sin(th2);];
SX G( zerosSX(3,1) );
makeDense(G);
G.at(0) = 0;
G.at(1) = -f1*sin(th1);
G.at(2) = -f2*sin(th2);
// H = [1;0;0;];
SX H( zerosSX(3,1) );
makeDense(H);
H.at(0) = 1;
H.at(1) = 0;
H.at(2) = 0;
// dy(1:3) = y(4:6);
xDot["th0"] = th0d;
xDot["th1"] = th1d;
xDot["th2"] = th2d;
// dy(4:6) = D\( - C*y(4:6) - G + H*u );
SX vel( zerosSX(3,1) );
makeDense(vel);
vel.at(0) = th0d;
vel.at(1) = th1d;
vel.at(2) = th2d;
SX accel = mul( inv(D), - mul( C, vel ) - G + mul( H, SX(action["u"]) ) );
simplify(accel.at(0));
simplify(accel.at(1));
simplify(accel.at(2));
xDot["th0d"] = accel.at(0);
xDot["th1d"] = accel.at(1);
xDot["th2d"] = accel.at(2);
// cout << th0 << endl;
// cout << th1 << endl;
// cout << th2 << endl;
// cout << th0d << endl;
// cout << th1d << endl;
// cout << th2d << endl;
// cout << accel.at(0) << endl;
// cout << accel.at(1) << endl;
// cout << accel.at(2) << endl;
outputs["cart_x"] = th0;
outputs["cart_y"] = 0;
outputs["bob0_x"] = th0 + L1*sin(th1);
outputs["bob0_y"] = - L1*cos(th1);
outputs["bob1_x"] = th0 + L1*sin(th1) + L2*sin(th2);
outputs["bob1_y"] = - L1*cos(th1) - L2*cos(th2);
}
