def arrived(desired, actual, threshold):
"""
returns True if the largest element-wise difference
between desired and actual is less than threshold, False otherwise
desired and actual should be lists of joint angles in degrees
"""
diff = orion5_math.absdiff(desired, actual)
diff[2] *= 0.75
diff[0] = abs((diff[0] + 180) % 360 - 180)
return max(diff) < threshold
def arrived(desired, actual, threshold):
diff = orion5_math.absdiff(joints, actual)
diff[0] = abs((diff[0] + 180) % 360 - 180)
return max(diff) < threshold