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 getValuesFromPose(self, P):
		'''return the virtual values of the pots corresponding to the pose P'''
		vals = []
		grads = []
		for i, r, l, placement, attach_p in zip(range(3), self.rs, self.ls, self.placements, self.attach_ps):
			#first pot axis
			a = placement.rot * col([1, 0, 0])
			#second pot axis
			b = placement.rot * col([0, 1, 0])
			#string axis
			c = placement.rot * col([0, 0, 1])

			#attach point on the joystick
			p_joystick = P * attach_p
			v = p_joystick - placement.trans
			va = v - dot(v, a)*a
			vb = v - dot(v, b)*b
			#angles of the pots
			alpha = math.atan2(dot(vb, a), dot(vb, c))
			beta = math.atan2(dot(va, b), dot(va, c))
			vals.append(alpha)
			vals.append(beta)
			
			#calculation of the derivatives
			dv = np.bmat([-P.rot.mat() * quat.skew(attach_p), P.rot.mat()])
			dva = (np.eye(3) - a*a.T) * dv
			dvb = (np.eye(3) - b*b.T) * dv
			dalpha = (1/dot(vb,vb)) * (dot(vb,c) * a.T - dot(vb,a) * c.T) * dvb
			dbeta = (1/dot(va,va)) * (dot(va,c) * b.T - dot(va,b) * c.T) * dva
			grads.append(dalpha)
			grads.append(dbeta)
		return (col(vals), np.bmat([[grads]]))
Exemple #3
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()
Exemple #4
0
	def __mul__(self, q):
		if(isinstance(q, Quaternion)):
			w = self.w * q.w - self.x() * q.x() - self.y() * q.y() - self.z() * q.z()
			x = self.w * q.x() + self.x() * q.w + self.y() * q.z() - self.z() * q.y()
			y = self.w * q.y() + self.y() * q.w + self.z() * q.x() - self.x() * q.z()
			z = self.w * q.z() + self.z() * q.w + self.x() * q.y() - self.y() * q.x()
			return Quaternion(w, col([x, y, z]))
		else:
			return (self * Quaternion(0, q) * self.inv()).im()
Exemple #5
0
	def readValues(self):
		'''read the values of the potentiometers'''
		self.ser.flushInput()
		self.ser.flushOutput()
		self.ser.write('g') #g for get
		raw_values = self.ser.readline()
		coef = self.pot_rad_per_unit
		values = col([coef * float(x) for x in raw_values.split()])
		return values
Exemple #6
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 #7
0
def id():
	return Quaternion(1., col([0., 0., 0.]))
Exemple #8
0
	def mat(self):
		return np.bmat([self * col([1,0,0]), self * col([0,1,0]), self * col([0,0,1])])