def setUp(self): self.dh_table = DH_TABLE self.dh_table['tool'] = None self.joints = (0, 0, 0, 0, 0, 0) self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp'] self.flange = forward_kinematics(*self.joints, **self.dh_table)['flange']
def __calibrate(self, num_points=-1, random=False): self.robot_infos = map(lambda x: forward_kinematics(*x, **dh_table), self.parser.robot_joints) fk = self.flange # anoto coords pentip_2d = self.parser.pen_pos # convert to mm, first point is origin #pentip_2d = (pentip_2d - pentip_2d[0])*0.3 f_ad = 7.0 * (25.4 / 600.0) pentip_2d = (pentip_2d) * f_ad if not num_points == -1: if random: unique_rand = list(set(randint(0, len(fk), num_points))) fk = fk[unique_rand] pentip_2d = pentip_2d[unique_rand] self.geometry_info = { 'data': { 'forward_kinematics': fk, 'pentip_2d': pentip_2d } } self._res, self.cond = calib.find_solution_pen_tip( self.geometry_info, num_points)
def main(): j1 = 0 j2 = 0 j3 = 0 j4 = 0 j5 = 0 j6 = 0 joint_values = [j1, j2, j3, j4, j5, j6] tool = hom(0, 0, 0, [0.1, 0, 0]) dh_table['tool'] = tool info = forward_kinematics(*joint_values, **dh_table) # multiplication from left: rotation in base to base # multiplication from right: rotation in tool to base tcp = info['tcp'].dot(hom(ori(-10, 30, 40))) pose = hom(tcp[:3, :3], [0.6, 0, 0.3]) s = calc_invkin_irb140(pose, raw_solutions=True) ik_up = forward_kinematics(*s[0], **dh_table) ik_down = forward_kinematics(*s[5], **dh_table) ik_up_back = forward_kinematics(*s[10], **dh_table) ik_down_back = forward_kinematics(*s[15], **dh_table) #plot_robot_geometry(info) plot_robot_geometry(ik_up, 'b') plot_robot_geometry(ik_up_back, 'b--') plot_robot_geometry(ik_down, 'r') plot_robot_geometry(ik_down_back, 'r--') xlabel('x [m]', fontsize=LABEL_SIZE) ylabel('z [m]', fontsize=LABEL_SIZE) xticks(fontsize=TICK_SIZE) yticks(fontsize=TICK_SIZE) grid() axes().set_aspect('equal', 'datalim') # save figure plot figpath = r"C:\Users\***REMOVED***\Dropbox\exjobb\rapport\images" savefig(path.join(figpath, "invkin-elbowupdown2.png"), bbox_inches='tight')
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 flange(pose): ''' Given a pose, calulate the forward-kinematics frame obtained from reaching the point by inverse-kinematics. ''' return forward_kinematics(*ik(pose), **dh_params)['flange']
'forward_kinematics': flanges, 'pentip_2d': penxyanoto, 'global_tool_orientation': penori } } # save data to pickle pickle_data = [] for k in xrange(len(flanges)): tmp = { 'pen_pos': geometry_info['data']['pentip_2d'][k], 'pen_ori': geometry_info['data']['global_tool_orientation'][k][:3,:3], 'pen_fsr': numpy.NAN, 'pen_fsr_adc': numpy.NAN, 'robot_joints': ik(global_tars[k]), 'robot_tcp': forward_kinematics(*global_tars[0], **dh_params)['tcp'], 'robot_flange': forward_kinematics(*global_tars[0], **dh_params)['flange'], } pickle_data.append(tmp) with open('calibration_verifitcation.pickle', 'wb+') as fp: pickle.dump(pickle_data, fp) with open('tool_wobj.yaml', 'w+') as fp: json.dump({ 'tool': { 'pos': list(dh_params['tool'][:3,3]), 'q': list(rot_to_quat(dh_params['tool'][:3,:3])) }, 'wobj': { 'pos': list(wobj[:3,3]),
def fk(pose): return forward_kinematics(*pose, **dh_params)
def setUp(self): self.dh_table = DH_TABLE self.dh_table['tool'] = hom(0, 0, 0, [0.1, 0.12, 0.13]) self.joints = (0, 0, 0, 0, 0, 0) self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
def robot_frames(*joints, **dh_table): return forward_kinematics(*joints, **dh_table)['robot_geometry_global']
def flange_from_joints(*joints, **dh_table): return forward_kinematics(*joints, **dh_table)['flange']
def tcp_from_joints(*joints, **dh_table): return forward_kinematics(*joints, **dh_table)['tcp']
def calc_robot_tcp(*joints): return forward_kinematics(*joints, **dh_table)['tcp']
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'])
tilt = numpy.linspace(-40,40) skew = numpy.linspace(0,0) angles = nzip(rot, tilt, skew) if __name__ == '__main__': dh_table['tool'] = hom(0,0,0,[0.0,0.0,0.1]) wobj = hom(-90,180,0,[0.3,0,0.5]) # generate a curve in the last global robot-frame curve = create_circle_curve(diameter=0.3) oris = orientation_frames(angles) frames = attach_frames(oris, curve) # tansform frames - paper -> robot trajectory = attach_to_base_frame(wobj, *frames) result = inverse_kinematics_curve(trajectory) path = find_single_path(result) print path for count in xrange(0,50,11): print count pl = StPlot() robot_info = forward_kinematics(*path[count], **dh_table) pl.draw_robot(robot_info['robot_geometry_global']) pl.draw_trajectory(trajectory) pl.draw_tool(robot_info['flange'], dh_table['tool']) pl.show() pl.draw_joint_paths(path)
j3 = rand_range(-230, 50) j4 = rand_range(-200, 200) j5 = rand_range(-115, 115) j6 = rand_range(-400, 400) j1 = 10 j2 = 20 j3 = 30 j4 = 40 j5 = 50 j6 = 60 joint_values = j1, j2, j3, j4, j5, j6 # get forward kinematics i.e. last global robot-frame robot_info = forward_kinematics(*joint_values, **DH_TABLE) T44 = robot_info['tcp'] IK_angles = inverse_kinematics_irb140(DH_TABLE, T44) # sanity check of forward kinematics for angles in IK_angles.T: info = forward_kinematics(*joint_values, **DH_TABLE) t44 = info['tcp'] assert (norm(T44 - t44) < 1e-7) # list of global-robot-frames global_robot_frames = construct_robot_geometry( info['robot_geometry_local']) # generate a curve in the last global robot-frame num_p = 50
def gen_robot_flanges(self): robot_infos = map(lambda x: forward_kinematics(*x, **dh_table), self.robot_joints) flange = mat([x['flange'] for x in robot_infos]) flange[:, :3, 3] = flange[:, :3, 3] * 1000 #convert to mm return flange