def pen_pos_pattern_mm_3d(self): """ Pen position relative to paper origin, which is the UL position of the pattern with crosses used in calibration. The coordinates are converted from ad to mm. """ return nmap(lambda x: x + [0], self.pen_pos_pattern_mm.tolist())
def targets(xmin,xmax,ymin,ymax, num): ''' Generates a ('num'X'num') rectangle grid of targets in the range of [xmin, xmax], [ymin, ymax]. Depending on application these can either be relative to a work boject or absolute. return: ndarray : (num**2, 4, 4) ''' x = linspace(xmin,xmax,num) y = linspace(ymin,ymax,num) xy = mat(mesh(x,y)).T res = nmap(lambda row: nmap(lambda pair: hom(0,0,0,[pair[0],pair[1],0]) ,row), xy) a,b,c,d = res.shape res = res.reshape((a*b,c,d)) return res
def jacobian_from_joints(*joints): fk = forward_kinematics(*joints, **DH_TABLE)['robot_geometry_global'] if not (DH_TABLE['tool'] is None): fk[-1] = fk[-1].dot(DH_TABLE['tool']) fk = mat(fk) zi = fk[:-1, :3, 2] Jw = zi.T on = fk[-1, :3, 3] oi = fk[:-1, :3, 3] odiff = on - oi Jv = nmap(lambda x: reduce(cross, x), zip(zi, odiff)).T J = mat([Jv, Jw]) J = J.reshape((6, 6)) return J
def calc_robot_curve_j1(): N = 100.0 T = 10.0 L = pi / 2.0 djs = linspace(0, L * 180 / pi, N) dts = linspace(0, T, N) djdts = djs / dts print djdts print djdts * dts dt = T / N # seconds / dx rads_per_second = L / T tcps = mat([ calc_robot_tcp(v, 10, 20, 0, 90, 0) for v in linspace(0, L * 180 / pi, N) ]) #w = -tcps[:,:3,2]*rads_per_second w = mat([[0, 0, 1] * int(N)]).reshape(N, 3) * rads_per_second print w.shape r = tcps[:, :3, 3] v = nmap(lambda x: reduce(n.cross, x), zip(w, r)) return tcps, nzip(v, w).reshape(N, 6), dts
tool = dh_params['tool'] # init - wobj wobj = hom(180-45,180,0,[0.2,-0.2,-0.2]) wobj[:3,:3] = wobj[:3,:3].dot(ori(0,180,90)) # init reference ref = zeros((3,4)) ref[:3,:3] = wobj[:3,:3] ref[:,3] = tool[:3,3] # init - paper targets tars = targets(0, 100e-3, 0, 200e-3, num=num_points_side) #tars = nmap(lambda x: x.dot(hom(ori(0,20,uniform(-180,180)))), tars) tars = nmap(lambda x: x[0].dot(hom(ori(x[1][0], x[1][1], x[1][2]))), zip(tars,angles)) # init - global targets global_tars = nmap(wobj.dot, tars) # calculate flanges from global targets flanges = nmap(flange, global_tars) flanges = nmap(lambda x: x + err_flange(0.03), flanges) # obtain relative pen positions from local targets # unit: meters penxy0 = nmap(pen_pos, tars) # convert to 'anoto coords' with projection error # unit: meters penxy = nmap(lambda x: pen_pos(x) + mat((-(x[2,0]/0.70710678118654757)*0.8e-3*2,
def main(): dh_table['tool'] = hom(0, 0, 0, [0.0, 0.0, 0.1]) wobj = hom(-90, 180, 0, [0.6, 0, 0.5]) # robot movement ##robot_movement_j1, vw, dts = calc_robot_curve_j1() ##trajectory = robot_movement_j1 curve = generate_symmetric_curve() N = len(curve) T = 12.56 L = 2 * pi dt = T / N # seconds / dx rads_per_second = L / T frame = calc_robot_tcp(0, 0, 0, 0, 90, 0) center = frame[:3, 3] curve = nmap(frame.dot, curve) r = curve[:, :3] - center frames = n.tile(frame, (50, 1)).reshape(50, 4, 4) frames[:, :, 3] = curve trajectory = frames t = linspace(0, T, N) w = mat([[0, 0, -1]] * len(trajectory)) * rads_per_second v = nmap(lambda x: reduce(n.cross, x), zip(w, r)) w = mat([[0, 0, 0]] * len(trajectory)) vw = n.hstack((v, w)) #inverse kinematics over curve result = inverse_kinematics_curve(trajectory) path = find_single_path(result) J = nmap(lambda x: jacobian_from_joints(*x), path) Jinv = nmap(inv, J) joint_angular_vel = nmap(lambda x: reduce(n.dot, x), zip(Jinv, vw)) * 180 / pi print 'Path found: \n {}'.format(path) print 'Joint angular velocities: \n {}'.format(joint_angular_vel) index = 0 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='lower right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q1.png', bbox_inches='tight', pad_inches=0) clf() index = 1 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='upper right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q2.png', bbox_inches='tight', pad_inches=0) clf() index = 2 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], loc='lower right', fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='lower right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q3.png', bbox_inches='tight', pad_inches=0) clf() index = 3 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='upper right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q4.png', bbox_inches='tight', pad_inches=0) clf() index = 4 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='lower right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q5.png', bbox_inches='tight', pad_inches=0) clf() index = 5 subplots_adjust(hspace=0.55) subplot(211) custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_Q, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE) subplot(212) custom_plot(n.round(joint_angular_vel[:, index], decimals=3), 'r', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel(YLABEL_W, fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) legend(['$\dot{q}_' + str(index + 1) + '$'], loc='lower right', fontsize=LEGEND_SIZE) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q6.png', bbox_inches='tight', pad_inches=0) clf() custom_plot(nmap(det, J), 'b', linewidth=1.5) xlabel(XLABEL, fontsize=LABEL_SIZE) ylabel('Jacobian determinant value', fontsize=LABEL_SIZE) xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) xlim((0, 49)) maximize_plot() savefig( 'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\J.png', bbox_inches='tight', pad_inches=0) clf() pl = StPlot() joints = path[13] robot_info = forward_kinematics(*joints, **dh_table) pl.draw_robot(robot_info['robot_geometry_global']) pl.draw_trajectory(trajectory) pl.draw_tool(robot_info['flange'], dh_table['tool'])
def attach_to_base_frame(base_frame, *frames): return nmap(lambda x: matmul(base_frame, x), frames)
"angerr1":[], "angerr2":[], "angerr32":[], } for i in xrange(1000): if i % 100 == 0: print(i) rts = ((rnd(-pi, pi), rnd(-pi/2.0, pi/2.0), rnd(-pi, pi)) for _ in range(100)) hom_oris = rotation_matrices(rts) x = ref = rts_mat(10,20,30) y = oris = hom_oris[:,:3,:3] z = nmap(ref.dot, oris) y = nmap(apply_noise, y) z = nmap(apply_noise, z) Y = y.reshape(300,3) Z = z.reshape(300,3) B = Y.T.dot(Y) C = Y.T.dot(Z) A = solve(B,C) U,s,V = svd(C) S = diag(s)
def pen_pos_pattern_wobj_global(self): return nmap(lambda x: self.yaml_wobj.dot(x), self.pen_pos_pattern_mm_hom)
def pen_pos_pattern_mm_hom(self): return nmap(lambda x: hom(0, 0, 0, x), self.pen_pos_pattern_mm_3d)
def flange_pos_error(self): return nmap(norm, self.flange_pos - self.parser.robot_flange)