# Commits

committed 2177320

Fix normal computation

• Participants
• Parent commits aaef81b

# File 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)`

# File PumaIK.py

` 	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 = PumaIK(True, True, (j1,j2,j3,j4,j5,j6))`
` `
` 	if len(sys.argv) != 3+1:`