puma / PumaFK.py

Diff from to

PumaFK.py

 		# equation 5
 		self.n = [
 			c1*tn1 - s1*tn2,
-			s1*tn1 - c1*tn2,
+			s1*tn1 + c1*tn2,
 			-s23*(c4*c5*c6 - s4*s6) - c23*s5*c6
 		]
 
 	j3 = Joint(0, 0, +90, -10500 - +11200, 0)
 	j4 = Joint(225, 0, -90, +17400, 0)
 	j5 = Joint(0, 0, 0, -8000 - +8000, -6917/30674.0)
-	j6 = Joint(50, 0, 0, -6500 - +6500, 1200/-10000.0)
+	j6 = Joint(50, 0, 0, +6500 - -6500, 1200/-10000.0)
 	r = PumaFK((j1,j2,j3,j4,j5,j6))
 
 	if len(sys.argv) == 6+1:
 	while True:
 		counts = MDC.get_counts()
 		r.update([int(x) for x in counts])
-		#print [int(x) for x in counts], [int(x) for x in r.p], [round(x,3) for x in r.a], [round(x,3) for x in r.s]
-		print [int(x) for x in counts], [int(x*180/pi) for x in r.theta]
+		print [int(x) for x in counts], [int(x) for x in r.p], [round(x,3) for x in r.s], [round(x,3) for x in r.n]
+		#print [int(x) for x in counts], [int(x*180/pi) for x in r.theta]
 		time.sleep(0.5)
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.