22 void forward(
const ConstVectorRef &x,
const ConstVectorRef &u,
24 Scalar rdot = x[0], phidot = x[1], theta = x[2], thetadot = x[3],
25 phi = x[4], posx = x[5], posy = x[6];
26 Scalar rdotdot = u[0], phidotdot = u[1];
28 data.xdot_[0] = rdotdot;
29 data.xdot_[1] = phidotdot;
30 data.xdot_[2] = thetadot;
32 std::cos(theta) * rdotdot /
length_;
33 data.xdot_[4] = phidot;
34 data.xdot_[5] = rdot * std::cos(phi);
35 data.xdot_[6] = rdot * std::sin(phi);
38 void dForward(
const ConstVectorRef &x,
const ConstVectorRef &u,
40 Scalar rdot = x[0], phidot = x[1], theta = x[2], thetadot = x[3],
41 phi = x[4], rdotdot = u[0];
46 std::sin(theta) * rdotdot /
length_;
49 data.Jx_(5, 0) = std::cos(phi);
50 data.Jx_(5, 4) = -rdot * std::sin(phi);
51 data.Jx_(6, 0) = std::sin(phi);
52 data.Jx_(6, 4) = rdot * std::cos(phi);
57 data.Ju_(3, 0) = -1 * std::cos(theta) /
length_;