Anonymous avatar Anonymous committed 42035e0

Fix joint 5 computation when joint 4 has 0 or 180 degree

Comments (0)

Files changed (1)

 		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
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.