22 void forward(
const ConstVectorRef &x,
const ConstVectorRef &u,
24 Scalar rdot = x[0], phidot = x[1], theta = x[2], thetadot = x[3],
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_;