Made changes to help with performance in demos
When deciding if changing the robot's trajectory should be made into a gradual process, we were comparing the current orientation with the next knot point's orientation. This was wrong. It should be comparing the current configuration with the orientation needed to drive towards the next knot point. This was fixed.
Reduced maximum velocity to 0.25m/s
Reduced threshold for gradual change to 0.2
Changed imminent collision time threshold to 2.5 seconds
Changed circle radius for collision detection to 0.75m.