Yeah the unstuck function needed better work. Maybe even switch it to something else instead of rigidbodies which could handle faster movement better?
