Fixed argument bug causing incorrect COM traj from zmp-lqr solution