Skip to content

Commit f089f53

Browse files
committed
handle values that are a few eps over 1 that cause acos domain errors, just clip the values
1 parent 2f513b0 commit f089f53

1 file changed

Lines changed: 3 additions & 3 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1035,7 +1035,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
10351035
rpy[2] = math.atan2(R[2, 1], R[1, 1]) # R+Y
10361036
else:
10371037
rpy[2] = -math.atan2(R[1, 0], R[2, 0]) # R-Y
1038-
rpy[1] = math.asin(R[0, 2])
1038+
rpy[1] = math.asin(np.clip(R[0, 2], -1.0, 1.0))
10391039
else:
10401040
rpy[0] = -math.atan2(R[0, 1], R[0, 0])
10411041
rpy[2] = -math.atan2(R[1, 2], R[2, 2])
@@ -1060,7 +1060,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
10601060
rpy[2] = -math.atan2(R[0, 1], R[0, 2]) # R-Y
10611061
else:
10621062
rpy[2] = math.atan2(-R[0, 1], -R[0, 2]) # R+Y
1063-
rpy[1] = -math.asin(R[2, 0])
1063+
rpy[1] = -math.asin(np.clip(R[2, 0], -1.0, 1.0))
10641064
else:
10651065
rpy[0] = math.atan2(R[2, 1], R[2, 2]) # R
10661066
rpy[2] = math.atan2(R[1, 0], R[0, 0]) # Y
@@ -1084,7 +1084,7 @@ def tr2rpy(T, unit='rad', order='zyx', check=False):
10841084
rpy[2] = -math.atan2(R[2, 0], R[0, 0]) # R-Y
10851085
else:
10861086
rpy[2] = math.atan2(-R[2, 0], -R[2, 1]) # R+Y
1087-
rpy[1] = -math.asin(R[1, 2]) # P
1087+
rpy[1] = -math.asin(np.clip(R[1, 2], -1.0, 1.0)) # P
10881088
else:
10891089
rpy[0] = math.atan2(R[1, 0], R[1, 1])
10901090
rpy[2] = math.atan2(R[0, 2], R[2, 2])

0 commit comments

Comments
 (0)