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)
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
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)
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()