MPC compare trajectory options

[avikde]

Closes #31

• implement lin along trajectory (time varying)
• other lin options

[avikde]

Sine traj

[avikde]

The implementation seems correct, but it goes unstable. Probably "constant input" over 10 timesteps results in a very unstable trajectory.

Instead treat the reference as a

• "get to desired velocity"
• desired vel can be modified using an outer loop for traj following if need be.
• current vel -> desired vel is a vector space, so interpolating seems like a fine option. could put acc limits on this "vel traj"

[avikde]

Tracking 0 vel from (1,0,0)

[avikde]

Can do a somersault! xr = np.array([0, 0, omega*t,0.,0.,omega])

This is a case where LQR fails