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 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]]))
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()
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()
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
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 id(): return Quaternion(1., col([0., 0., 0.]))
def mat(self): return np.bmat([self * col([1,0,0]), self * col([0,1,0]), self * col([0,0,1])])
