- Add parameter to stop simulation when specified index in reference trajectory is reached - Modify `forwardIntegrate` to return `Xobs` for plotting - Plot obstacles in part2 testing figure - Change some line plotting styles for clarity