make robust to unreachable point
@@ -282,13 +282,15 @@ class Puma560(DHRobot):
r = np.sqrt(V114**2 + Pz**2)
+ with np.errstate(invalid='raise'):
+ try:
Psi = np.arccos(
(a2**2 - d4**2 - a3**2 + V114**2 + Pz**2)
/ (2.0 * a2 * r))
+ except FloatingPointError:
+ return "Out of reach"
+
- if np.isnan(Psi):
- theta = None # pragma nocover
- else:
theta[1] = np.arctan2(Pz, V114) + n2 * Psi
# Solve for theta[2]
