Source

puma / PumaIK.py

Diff from to

File PumaIK.py

 		C4 = M * (C1*C23*ax + S1*C23*ay- S23*az)
 
 		self.theta[3] = atan2(S4, C4)
+		print "S4=" + str(S4) + " sin(4)=" + str(sin(self.theta[3]))
+		print "C4=" + str(C4) + " cos(4)=" + str(cos(self.theta[3]))
+
+		# Replace S4/C4 with the computed values based on theta4
+		# Degenerate cases might have a bad value in C4 if S4 is zero
+		S4 = sin(self.theta[3])
+		C4 = cos(self.theta[3])
 
 		S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az
 		C5 = C1*S23*ax + S1*S23*ay + C23*az