def SetInit2Start(self,T0_TSY): T_rh = None T_lf = None # Defines a box in which to have the end-effector manipulate if self.use_manipbox : # hand_box = matrix( self.manipbox_dim + [-1000,1000,-1000,1000,-1000,1000] ) hand_box = matrix( self.manipbox_dim + [-pi,pi,-pi,pi,-pi,pi] ) T_rh = dot(T0_TSY, MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0])))) # T0_TSY T_lh = dot(T0_TSY, MakeTransform(rodrigues([pi/2,0,0]),transpose(matrix([0,0,0])))) # T0_TSY else : hand_box = matrix([-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000]) T_rh = self.robotManips[1].GetEndEffectorTransform() T_lh = self.robotManips[0].GetEndEffectorTransform() # Define Task Space Region strings # Left Hand TSRStringLH1 = SerializeTSR(0,'NULL',T_lh,eye(4),hand_box) TSRStringLH0 = SerializeTSR(0,'NULL',self.robotManips[0].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # Right Hand TSRStringRH1 = SerializeTSR(1,'NULL',T_rh,eye(4),hand_box) TSRStringRH0 = SerializeTSR(1,'NULL',self.robotManips[1].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # Define TSR for this path # Left Foot TSRStringLF1 = SerializeTSR(2,'NULL', self.robotManips[2].GetEndEffectorTransform(), eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # Right Foot TSRStringRF1 = SerializeTSR(3,'NULL', self.robotManips[3].GetEndEffectorTransform(), eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # Head TSRStringH = SerializeTSR(4,'NULL', self.robotManips[4].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # We defined Task Space Regions. Now let's concatenate them. self.TSRChainStringFeetandHead_init2start_bh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[]) self.TSRChainStringFeetandHead_init2start_lh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[]) self.TSRChainStringFeetandHead_init2start_rh = SerializeTSRChain(0,0,1,1,TSRStringLH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
def extr_TSR(self, window_points, ee_trans, hold, stud_offset=[], plot=False, draw_axes=False, verbose=False, eigenvalue_threshold=0.0001): eig_pairs = self.run_PCA(window_points) matrix_w = np.hstack( (eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1))) if verbose: print 'New coordinates:\n', matrix_w transformed = matrix_w.T.dot(window_points.T) if plot: fig = plt.figure(2) #fig = plt.figure(figsize=(7, 7)) ax = fig.add_subplot(111, projection='3d') plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2], 'o', markersize=7, color='blue', alpha=0.5) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Transformed samples with class labels') #plt.show(block=False) plt.draw() # print np.min(transformed.T[:, 0]) # print np.max(transformed.T[:, 0]) # print np.min(transformed.T[:, 1]) # print np.max(transformed.T[:, 1]) # print np.min(transformed.T[:, 2]) # print np.max(transformed.T[:, 2]) # Path TSR frame TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :])) change_axes = MakeTransform(rodrigues([0, pi / 2, 0]), matrix([ 0, 0, 0 ])) * MakeTransform(rodrigues([0, 0, pi / 2]), matrix([0, 0, 0])) TSR_0_w_path = TSR_0_w_path * change_axes # Path EE offset EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0])) # Path B_w # print eig_pairs[0][0] # print eig_pairs[1][0] # print eig_pairs[2][0] # print eigenvalue_threshold # TODO fix the frames from extraction to execution z_min = np.min(transformed.T[:, 0]) - transformed[0, 0] - 0. if ( eig_pairs[0][0] < eigenvalue_threshold) else -1000 z_max = np.max(transformed.T[:, 0]) - transformed[0, 0] + 0. if ( eig_pairs[0][0] < eigenvalue_threshold) else 1000 x_min = np.min(transformed.T[:, 1]) - transformed[1, 0] - 0. if ( eig_pairs[1][0] < eigenvalue_threshold) else -1000 x_max = np.max(transformed.T[:, 1]) - transformed[1, 0] + 0. if ( eig_pairs[1][0] < eigenvalue_threshold) else 1000 y_min = np.min(transformed.T[:, 2]) - transformed[2, 0] - 0.2 if ( eig_pairs[2][0] < eigenvalue_threshold) else -1000 y_max = np.max(transformed.T[:, 2]) - transformed[2, 0] + 0.2 if ( eig_pairs[2][0] < eigenvalue_threshold) else 1000 B_w_path = mat([ x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000 ]) #print window_points[0, :] #print transformed[:, 0] #print B_w_path #sys.stdin.readline() # Goal TSR frame TSR_0_w_goal = ee_trans[-1] # Goal EE offset #EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0])) EE_offset_goal = stud_offset[-1] # for the screw task the offset # Goal B_w B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0]) if draw_axes: #draw TSR_frame self.drawingHandles.append( misc.DrawAxes(self.env, TSR_0_w_path, 0.3)) self.drawingHandles.append( misc.DrawAxes(self.env, TSR_0_w_goal, 0.3)) new_z_axis = eig_pairs[0][1] plt.show(block=False) return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path ], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def run_PCA(window_points, ee_trans, hold, stud_offset=[], plot=False, draw_axes=False, verbose=False, eigenvalue_threshold=0.001): # 1. PCA calculations mean_x = np.mean(window_points[:, 0]) mean_y = np.mean(window_points[:, 1]) mean_z = np.mean(window_points[:, 2]) mean_vector = np.array([[mean_x], [mean_y], [mean_z]]) if verbose: print 'Mean Vector:\n', mean_vector dim = 3 scatter_matrix = np.zeros((dim, dim)) for i in range(window_points.shape[0]): scatter_matrix += (window_points[i, :].reshape(dim, 1) - mean_vector).dot( (window_points[i, :].reshape(dim, 1) - mean_vector).T) if verbose: print 'Scatter Matrix:\n', scatter_matrix cov_mat = np.cov( [window_points[:, 0], window_points[:, 1], window_points[:, 2]]) if verbose: print 'Covariance Matrix:\n', cov_mat # eigenvectors and eigenvalues for the from the scatter matrix eig_val_sc, eig_vec_sc = np.linalg.eigh(scatter_matrix) # eigenvectors and eigenvalues for the from the covariance matrix eig_val_cov, eig_vec_cov = np.linalg.eig(cov_mat) for i in range(len(eig_val_sc)): eigvec_sc = eig_vec_sc[:, i] #.reshape(1, 3).T eigvec_cov = eig_vec_cov[:, i] #.reshape(1, 3).T assert eigvec_sc.all() == eigvec_cov.all( ), 'Eigenvectors are not identical' if verbose: print 'Eigenvector {}: \n{}'.format(i + 1, eig_vec_sc[:, i]) if verbose: print 'Eigenvalue {} from scatter matrix: {}'.format( i + 1, eig_val_sc[i]) if verbose: print 'Eigenvalue {} from covariance matrix: {}'.format( i + 1, eig_val_cov[i]) if verbose: print 'Scaling factor: ', eig_val_sc[i] / eig_val_cov[i] if verbose: print 40 * '-' # check eigenvector-eigenvalue calculations for i in range(len(eig_val_sc)): eigv = eig_vec_sc[:, i].reshape(1, dim).T np.testing.assert_array_almost_equal(scatter_matrix.dot(eigv), eig_val_sc[i] * eigv, decimal=6, err_msg='', verbose=True) if plot: fig = plt.figure(1) #fig = plt.figure(figsize=(7, 7)) ax = fig.add_subplot(111, projection='3d') plt.plot(window_points[:, 0], window_points[:, 1], window_points[:, 2], 'o', markersize=8, color='green', alpha=0.2) plt.plot([mean_x], [mean_y], [mean_z], 'o', markersize=10, color='red', alpha=0.5) first = True for v in eig_vec_sc.T: c = 'b' if first: c = 'r' first = False a = Arrow3D([window_points[0, 0], v[0] + window_points[0, 0]], [window_points[0, 1], v[1] + window_points[0, 1]], [window_points[0, 2], v[2] + window_points[0, 2]], mutation_scale=1, lw=3, arrowstyle="-|>", color=c) ax.add_artist(a) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Eigenvectors') plt.draw() # test if the eigenvectors are unit vectors # for ev in eig_vec_sc: # np.testing.assert_array_almost_equal(1.0, np.linalg.norm(ev)) # Make a list of (eigenvalue, eigenvector) tuples eig_pairs = [(np.abs(eig_val_sc[i]), eig_vec_sc[:, i]) for i in range(len(eig_val_sc))] # Sort the (eigenvalue, eigenvector) tuples from high to low eig_pairs.sort() eig_pairs.reverse() matrix_w = np.hstack( (eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1))) if verbose: print 'New coordinates:\n', matrix_w transformed = matrix_w.T.dot(window_points.T) if plot: fig = plt.figure(2) ax = fig.add_subplot(111, projection='3d') plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2], 'o', markersize=7, color='blue', alpha=0.5) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Transformed samples with class labels') plt.draw() # 2. TSR calculations from PCA results # Path TSR frame TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :])) change_axes = MakeTransform(rodrigues( [0, pi / 2, 0]), matrix([0, 0, 0])) * MakeTransform( rodrigues([0, 0, pi / 2]), matrix([0, 0, 0])) TSR_0_w_path = TSR_0_w_path * change_axes # check for valid rotation matrix (det = 1) and fix if numpy.linalg.det(TSR_0_w_path[0:3][:, 0:3]) != 1: TSR_0_w_path[0:3, 0] *= -1 # Path EE offset EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0])) # Path B_w if verbose: print "eigen pairs" print eig_pairs[0][0] print eig_pairs[1][0] print eig_pairs[2][0] # print eigenvalue_threshold # TODO fix the frames from extraction to execution z_min = np.min(transformed.T[:, 0]) - transformed[0, 0] - 0. if ( abs(eig_pairs[0][0]) < eigenvalue_threshold) else -1000 z_max = np.max(transformed.T[:, 0]) - transformed[0, 0] + 0. if ( abs(eig_pairs[0][0]) < eigenvalue_threshold) else 1000 x_min = np.min(transformed.T[:, 1]) - transformed[1, 0] - 0. if ( abs(eig_pairs[1][0]) < eigenvalue_threshold) else -1000 x_max = np.max(transformed.T[:, 1]) - transformed[1, 0] + 0. if ( abs(eig_pairs[1][0]) < eigenvalue_threshold) else 1000 y_min = np.min(transformed.T[:, 2]) - transformed[2, 0] - 0. if ( abs(eig_pairs[2][0]) < eigenvalue_threshold) else -1000 y_max = np.max(transformed.T[:, 2]) - transformed[2, 0] + 0. if ( abs(eig_pairs[2][0]) < eigenvalue_threshold) else 1000 B_w_path = mat([ x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000 ]) if verbose: print "boundaries:" print transformed print B_w_path # Goal TSR frame #TSR_0_w_goal = ee_trans[-1] # not gonna be used TSR_0_w_goal = ee_trans[0] # Goal EE offset #EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0])) EE_offset_goal = ee_trans[ -1] #MakeTransform(eye(3), matrix([0, -0.05, -0.002])) # for the unscrew nut task (took from AppraochAction) #EE_offset_goal = stud_offset[-1] # for the screw task the offset # Goal B_w B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0]) if draw_axes: #draw TSR_frame #drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.035, linewidth=1.0)) drawingHandles.append( misc.DrawAxes(env, TSR_0_w_path, 0.15, linewidth=1.0)) #drawingHandles.append(misc.DrawAxes(env, TSR_0_w_goal, 0.3)) new_z_axis = eig_pairs[0][1] plt.show(block=False) return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def extr_TSR(self, window_points, ee_trans, hold, stud_offset=[], plot = False, draw_axes = False, verbose = False, eigenvalue_threshold = 0.0001): eig_pairs = self.run_PCA(window_points) matrix_w = np.hstack((eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1))) if verbose: print 'New coordinates:\n', matrix_w transformed = matrix_w.T.dot(window_points.T) if plot: fig = plt.figure(2) #fig = plt.figure(figsize=(7, 7)) ax = fig.add_subplot(111, projection='3d') plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2], 'o', markersize=7, color='blue', alpha=0.5) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Transformed samples with class labels') #plt.show(block=False) plt.draw() # print np.min(transformed.T[:, 0]) # print np.max(transformed.T[:, 0]) # print np.min(transformed.T[:, 1]) # print np.max(transformed.T[:, 1]) # print np.min(transformed.T[:, 2]) # print np.max(transformed.T[:, 2]) # Path TSR frame TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :])) change_axes = MakeTransform(rodrigues([0, pi/2, 0]), matrix([0, 0, 0]))*MakeTransform(rodrigues([0, 0, pi/2]), matrix([0, 0, 0])) TSR_0_w_path = TSR_0_w_path*change_axes # Path EE offset EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0])) # Path B_w # print eig_pairs[0][0] # print eig_pairs[1][0] # print eig_pairs[2][0] # print eigenvalue_threshold # TODO fix the frames from extraction to execution z_min = np.min(transformed.T[:, 0])-transformed[0, 0]-0. if (eig_pairs[0][0] < eigenvalue_threshold) else -1000 z_max = np.max(transformed.T[:, 0])-transformed[0, 0]+0. if (eig_pairs[0][0] < eigenvalue_threshold) else 1000 x_min = np.min(transformed.T[:, 1])-transformed[1, 0]-0. if (eig_pairs[1][0] < eigenvalue_threshold) else -1000 x_max = np.max(transformed.T[:, 1])-transformed[1, 0]+0. if (eig_pairs[1][0] < eigenvalue_threshold) else 1000 y_min = np.min(transformed.T[:, 2])-transformed[2, 0]-0.2 if (eig_pairs[2][0] < eigenvalue_threshold) else -1000 y_max = np.max(transformed.T[:, 2])-transformed[2, 0]+0.2 if (eig_pairs[2][0] < eigenvalue_threshold) else 1000 B_w_path = mat([x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000]) #print window_points[0, :] #print transformed[:, 0] #print B_w_path #sys.stdin.readline() # Goal TSR frame TSR_0_w_goal = ee_trans[-1] # Goal EE offset #EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0])) EE_offset_goal = stud_offset[-1] # for the screw task the offset # Goal B_w B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0]) if draw_axes: #draw TSR_frame self.drawingHandles.append(misc.DrawAxes(self.env, TSR_0_w_path, 0.3)) self.drawingHandles.append(misc.DrawAxes(self.env, TSR_0_w_goal, 0.3)) new_z_axis = eig_pairs[0][1] plt.show(block = False) return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]
def run_PCA(window_points, ee_trans, hold, stud_offset=[], plot = False, draw_axes = False, verbose = False, eigenvalue_threshold = 0.001): # 1. PCA calculations mean_x = np.mean(window_points[:, 0]) mean_y = np.mean(window_points[:, 1]) mean_z = np.mean(window_points[:, 2]) mean_vector = np.array([[mean_x], [mean_y], [mean_z]]) if verbose: print 'Mean Vector:\n', mean_vector dim = 3 scatter_matrix = np.zeros((dim, dim)) for i in range(window_points.shape[0]): scatter_matrix += (window_points[i, :].reshape(dim, 1) - mean_vector).dot((window_points[i, :].reshape(dim, 1) - mean_vector).T) if verbose: print 'Scatter Matrix:\n', scatter_matrix cov_mat = np.cov([window_points[:, 0], window_points[:, 1], window_points[:, 2]]) if verbose: print 'Covariance Matrix:\n', cov_mat # eigenvectors and eigenvalues for the from the scatter matrix eig_val_sc, eig_vec_sc = np.linalg.eigh(scatter_matrix) # eigenvectors and eigenvalues for the from the covariance matrix eig_val_cov, eig_vec_cov = np.linalg.eig(cov_mat) for i in range(len(eig_val_sc)): eigvec_sc = eig_vec_sc[:, i]#.reshape(1, 3).T eigvec_cov = eig_vec_cov[:, i]#.reshape(1, 3).T assert eigvec_sc.all() == eigvec_cov.all(), 'Eigenvectors are not identical' if verbose: print 'Eigenvector {}: \n{}'.format(i+1, eig_vec_sc[:, i]) if verbose: print 'Eigenvalue {} from scatter matrix: {}'.format(i+1, eig_val_sc[i]) if verbose: print 'Eigenvalue {} from covariance matrix: {}'.format(i+1, eig_val_cov[i]) if verbose: print 'Scaling factor: ', eig_val_sc[i]/eig_val_cov[i] if verbose: print 40 * '-' # check eigenvector-eigenvalue calculations for i in range(len(eig_val_sc)): eigv = eig_vec_sc[:, i].reshape(1, dim).T np.testing.assert_array_almost_equal(scatter_matrix.dot(eigv), eig_val_sc[i] * eigv, decimal=6, err_msg='', verbose=True) if plot: fig = plt.figure(1) #fig = plt.figure(figsize=(7, 7)) ax = fig.add_subplot(111, projection='3d') plt.plot(window_points[:, 0], window_points[:, 1], window_points[:, 2], 'o', markersize=8, color='green', alpha=0.2) plt.plot([mean_x], [mean_y], [mean_z], 'o', markersize=10, color='red', alpha=0.5) first = True for v in eig_vec_sc.T: c = 'b' if first: c = 'r' first = False a = Arrow3D([window_points[0, 0], v[0]+window_points[0, 0]], [window_points[0, 1], v[1]+window_points[0, 1]], [window_points[0, 2], v[2]+window_points[0, 2]], mutation_scale=1, lw=3, arrowstyle="-|>", color=c) ax.add_artist(a) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Eigenvectors') plt.draw() # test if the eigenvectors are unit vectors # for ev in eig_vec_sc: # np.testing.assert_array_almost_equal(1.0, np.linalg.norm(ev)) # Make a list of (eigenvalue, eigenvector) tuples eig_pairs = [(np.abs(eig_val_sc[i]), eig_vec_sc[:, i]) for i in range(len(eig_val_sc))] # Sort the (eigenvalue, eigenvector) tuples from high to low eig_pairs.sort() eig_pairs.reverse() matrix_w = np.hstack((eig_pairs[0][1].reshape(3, 1), eig_pairs[1][1].reshape(3, 1), eig_pairs[2][1].reshape(3, 1))) if verbose: print 'New coordinates:\n', matrix_w transformed = matrix_w.T.dot(window_points.T) if plot: fig = plt.figure(2) ax = fig.add_subplot(111, projection='3d') plt.plot(transformed.T[:, 0], transformed.T[:, 1], transformed.T[:, 2], 'o', markersize=7, color='blue', alpha=0.5) ax.set_xlabel('x_values') ax.set_ylabel('y_values') ax.set_zlabel('z_values') plt.title('Transformed samples with class labels') plt.draw() # 2. TSR calculations from PCA results # Path TSR frame TSR_0_w_path = MakeTransform(matrix_w, matrix(window_points[0, :])) change_axes = MakeTransform(rodrigues([0, pi/2, 0]), matrix([0, 0, 0]))*MakeTransform(rodrigues([0, 0, pi/2]), matrix([0, 0, 0])) TSR_0_w_path = TSR_0_w_path*change_axes # check for valid rotation matrix (det = 1) and fix if numpy.linalg.det(TSR_0_w_path[0:3][:,0:3]) != 1: TSR_0_w_path[0:3, 0] *= -1; # Path EE offset EE_offset_path = MakeTransform(eye(3), matrix([0, 0, 0])) # Path B_w if verbose: print "eigen pairs" print eig_pairs[0][0] print eig_pairs[1][0] print eig_pairs[2][0] # print eigenvalue_threshold # TODO fix the frames from extraction to execution z_min = np.min(transformed.T[:, 0])-transformed[0, 0]-0. if (abs(eig_pairs[0][0]) < eigenvalue_threshold) else -1000 z_max = np.max(transformed.T[:, 0])-transformed[0, 0]+0. if (abs(eig_pairs[0][0]) < eigenvalue_threshold) else 1000 x_min = np.min(transformed.T[:, 1])-transformed[1, 0]-0. if (abs(eig_pairs[1][0]) < eigenvalue_threshold) else -1000 x_max = np.max(transformed.T[:, 1])-transformed[1, 0]+0. if (abs(eig_pairs[1][0]) < eigenvalue_threshold) else 1000 y_min = np.min(transformed.T[:, 2])-transformed[2, 0]-0. if (abs(eig_pairs[2][0]) < eigenvalue_threshold) else -1000 y_max = np.max(transformed.T[:, 2])-transformed[2, 0]+0. if (abs(eig_pairs[2][0]) < eigenvalue_threshold) else 1000 B_w_path = mat([x_min, x_max, y_min, y_max, z_min, z_max, -1000, 1000, -1000, 1000, -1000, 1000]) if verbose: print "boundaries:" print transformed print B_w_path # Goal TSR frame #TSR_0_w_goal = ee_trans[-1] # not gonna be used TSR_0_w_goal = ee_trans[0] # Goal EE offset #EE_offset_goal = MakeTransform(eye(3), matrix([0, 0, 0])) EE_offset_goal = ee_trans[-1] #MakeTransform(eye(3), matrix([0, -0.05, -0.002])) # for the unscrew nut task (took from AppraochAction) #EE_offset_goal = stud_offset[-1] # for the screw task the offset # Goal B_w B_w_goal = mat([-0, 0, -0, 0, -0, 0, -0, 0, -0, 0, -0, 0]) if draw_axes: #draw TSR_frame #drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.035, linewidth=1.0)) drawingHandles.append(misc.DrawAxes(env, TSR_0_w_path, 0.15, linewidth=1.0)) #drawingHandles.append(misc.DrawAxes(env, TSR_0_w_goal, 0.3)) new_z_axis = eig_pairs[0][1] plt.show(block = False) return new_z_axis, [TSR_0_w_path, EE_offset_path, B_w_path], [TSR_0_w_goal, EE_offset_goal, B_w_goal]