Added function to get length that the end effector travels in the path
Fixed it up so that the dist is returned properly...may not be perfect yet.