Torque trajectory planning