minor update

Author: chenxi-wang

graspnetAPI/utils/rotation.py

@@ -55,7 +55,7 @@ def matrix_to_dexnet_params(matrix):
     R = np.c_[axis_x, np.c_[axis_y, axis_z]]
     approach = R.T.dot(approach)
     cos_t, sin_t = approach[0], -approach[2]
-    angle = np.arccos(cos_t)
+    angle = np.arccos(max(min(cos_t,1),-1))
     if sin_t < 0:
         angle = pi * 2 - angle
     return binormal, angle
@@ -139,4 +139,4 @@ def dexnet_params_to_matrix(binormal, angle):
                   [-np.sin(angle), 0, np.cos(angle)]])
     R2 = np.c_[axis_x, np.c_[axis_y, axis_z]]
     matrix = R2.dot(R1)
-    return matrix
+    return matrix