I have fixed the plotting error and add acceleration limit to the plot. I haven't made all figures in one since by that way, plotting will be too slow to catch the chrono's update frequency. There is one problem remained, that's when update the predicted trajectory, the trajectory will blink. This is because I need to clean the previous plot so that the new plot will not overlap with it. I will find a smarter way but the current version can still work.
I have fixed the plotting error and add acceleration limit to the plot. I haven't made all figures in one since by that way, plotting will be too slow to catch the chrono's update frequency. There is one problem remained, that's when update the predicted trajectory, the trajectory will blink. This is because I need to clean the previous plot so that the new plot will not overlap with it. I will find a smarter way but the current version can still work.