update 完成复位等逻辑

Util/util_math.py

@@ -0,0 +1,11 @@
+import math
+
+
+def get_distance(p1, p2):
+    """
+    计算两点间的距离
+    :param p1:
+    :param p2:
+    :return:
+    """
+    return math.sqrt((p1.X - p2.X) ** 2 + (p1.Y - p2.Y) ** 2+ (p1.Z - p2.Z)**2)