Made changes to help scenario where the robots get too close to one another and stop to look for a feasible path, but remain stuck because their current configurations are within the collision detection range.
Changed time_until_collision threshold to stop the robots to 4 seconds
Changed trajectory point sampling to test collision to every 4 points
Changed collision circle radius to 0.4m