Make time for between-contour-trajectories realistic
Currently, between contour trajectories use a fixed timestep for each waypoint in the trajectory. Change this to a realistic time based off distance in c-space.
-
Get time steps by a multiple of distance in space -
Use large multiplier with real hardware and decrease until fast but safe speed for movement -
Use this as the default multiplier for planning