Add method to check collision of interpolated motions