simple but working traj opt