Exemple #1
0
	def __init__(self):
		self.ser = serial.Serial('/dev/ttyACM0', 9600)

		#geometrical calibration
		self.rs = [50, 50, 50]
		self.ls = [95, 130, 95]
		self.pot_rad_per_unit = 1./3000.*math.pi
		angles = [2./3.*math.pi, 0., -2./3.*math.pi]

		#placements of the 3 joysticks
		self.placements = []
		#attach point on the ball
		self.attach_ps = []
		for r,l,a in zip(self.rs, self.ls, angles):
			p_init = pose.exp(col([0, 0, 0, 0, 0, -(r+l)]))
			p_rot = pose.exp(col([0, a, 0, 0, 0, 0]))
			placement = p_rot * p_init
			self.placements.append(placement)
			attach_p = placement * col([0, 0, l])
			self.attach_ps.append(attach_p)

		#last calculated pose in logarithmic coordinates
		self.last_x = col([0, 0, 0, 0, 0, 0])
		#definition of the numerical solver
		f = lambda x: self.getValuesFromPose(pose.exp(x))
		self.solver = solver.Solver(f)
Exemple #2
0
	def getPose(self):
		'''return the pose of the joystick (numerically calculated)'''
		y = self.readValues()
		x = self.solver.solve(y, self.last_x)
		P = pose.exp(x)
		self.last_x = pose.log(P)
		return P
Exemple #3
0
	def getNumericalGradient(self, P, h = 1e-5):
		'''just to check the calculations...'''
		grad = []
		for i in range(6):
			dv = [0, 0, 0, 0, 0, 0]
			dv[i] = h
			gi = (1./h) * (self.getValuesFromPose(P * pose.exp(col(dv))) - self.getValuesFromPose(P))
			grad.append(gi)
		return np.bmat(grad)
Exemple #4
0
	def run(self):

		while 1:
			for event in pygame.event.get():
				if event.type == pygame.QUIT:
					pygame.quit()
					sys.exit()

			#40 fps max
			time_passed = self.clock.tick(40)

			self.screen.fill((0,32,0))
		
			#events from joystick
			P = pose.exp(col([-math.pi / 2,0,0,0,0,0])) * self.joystick.getPose()

			# It will hold transformed vertices.
			t = []
			
			for v in self.vertices:
				v_c = col(v)
				p = 0.01 * (P * (100 * v_c))				
				# Transform the point from 3D to 2D
				factor = 256 / (4 + p[2,0])
				x = p[0,0] * factor + self.screen.get_width() / 2
				y = -p[1,0] * factor + self.screen.get_height() / 2
				z = p[2,0]
				# Put the point in the list of transformed vertices
				t.append([x,y,z])

			# Calculate the average Z values of each face.
			avg_z = []
			i = 0
			for f in self.faces:
				z = (t[f[0]][2] + t[f[1]][2] + t[f[2]][2] + t[f[3]][2]) / 4.0
				avg_z.append([i,z])
				i = i + 1

			# Draw the faces using the Painter's algorithm:
			# Distant faces are drawn before the closer ones.
			for tmp in sorted(avg_z,key=itemgetter(1),reverse=True):
				face_index = tmp[0]
				f = self.faces[face_index]
				pointlist = [(t[f[0]][0], t[f[0]][1]), (t[f[1]][0], t[f[1]][1]),
							 (t[f[1]][0], t[f[1]][1]), (t[f[2]][0], t[f[2]][1]),
							 (t[f[2]][0], t[f[2]][1]), (t[f[3]][0], t[f[3]][1]),
							 (t[f[3]][0], t[f[3]][1]), (t[f[0]][0], t[f[0]][1])]
				pygame.draw.polygon(self.screen,self.colors[face_index],pointlist)

				
			self.angle += 1
			
			pygame.display.flip()