def denavit_hartenberg(dh_table, show_info): """ Get transformation matrix from Denavit Hartenberg input table :param dh_matrix: main denavit hartenberg table size(N, 4) :param show_info: show information ("yes" or "no") :returns: transformation matrix size(4, 4) """ TM = np.identity((4)) # Get transformation matrix for each DH row (and keep multiplying it) for i in range(np.size(dh_table, 0)): TMi_0 = transf.Transformation(dh_table[i, 0], 0, 0, [0, 0, 0]).TM TMi_1 = transf.Transformation(0, 0, 0, [dh_table[i, 1], 0, 0]).TM TMi_2 = transf.Transformation(0, 0, 0, [0, 0, dh_table[i, 2]]).TM TMi_3 = transf.Transformation(0, 0, dh_table[i, 3], [0, 0, 0]).TM TM_current = np.dot(np.dot(np.dot(TMi_0, TMi_1), TMi_2), TMi_3) TM = np.dot(TM, TM_current) if (show_info == "yes" or show_info == 1): print("TMi_0 -", i, "\n", TMi_0) print("TMi_1 -", i, "\n", TMi_1) print("TMi_2 -", i, "\n", TMi_2) print("TMi_3 -", i, "\n", TMi_3) print("TM_current -", i, "\n", TM_current) print("TM -", i, "\n", TM, "\n\n") return TM
def transform(self): '''To apply loop transformations on the annotated code''' # parse the code to get the AST stmts = parser.getParser(self.line_no).parse(self.module_body_code) if isinstance(stmts[0], ast.TransformStmt) and stmts[0].stmt is None: # transform the enclosed annot_body_code annotated_stmts = parser.getParser(self.line_no).parse( self.annot_body_code) if len(annotated_stmts) == 1: annotated_stmt = annotated_stmts[0] else: annotated_stmt = ast.CompStmt(annotated_stmts[0]) stmts[0].stmt = annotated_stmt # apply transformations t = transformation.Transformation(self.perf_params, self.verbose, self.language, self.tinfo) transformed_stmts = t.transform(stmts) # generate code for the transformed ASTs indent = ' ' * self.indent_size extra_indent = ' ' cgen = codegen.CodeGen(self.language) transformed_code = '\n' for s in transformed_stmts: transformed_code += cgen.generate(s, indent, extra_indent) # return the transformed code return transformed_code
def align_transformations(match_what, match_to): def objective(x): t = transformation.from_array(x) return np.sum(match_what.change_basis(t).disparity(match_to)) constraint = { "type" : "eq", "fun" : (lambda x: (np.linalg.norm(x[:4]) - 1) ** 2) } x0 = transformation.Transformation().as_array() result = scipy.optimize.minimize(objective, x0, constraints=constraint) return transformation.from_array(result.x)
def tileForRegs(self, loops, ufactors, stmt): '''To apply register tiling transformation''' # perform the register tiling transformation t = transformation.Transformation(loops, ufactors, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def permute(self, seq, stmt): '''To apply loop permutation/interchange transformation''' # perform the loop permutation transformation t = transformation.Transformation(seq, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def optimizeArrayCopy(self, aref, suffix, dtype, dimsizes, stmt): '''To apply array copy optimization''' # perform the bound replacement transformation t = transformation.Transformation(aref, suffix, dtype, dimsizes, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def replaceScalars(self, dtype, prefix, stmt): '''To apply scalar replacement transformation''' # perform the scalar replacement transformation t = transformation.Transformation(dtype, prefix, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def tile(self, tsize, tindex, stmt): '''To apply loop tiling transformation''' # perform the loop tiling transformation t = transformation.Transformation(tsize, tindex, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def replaceBounds(self, lprefix, uprefix, stmt): '''To apply bound replacement transformation''' # perform the bound replacement transformation t = transformation.Transformation(lprefix, uprefix, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def insertPragmas(self, pragmas, stmt): '''To apply pragma directive insertion''' # perform the pragma directive insertion t = transformation.Transformation(pragmas, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def unrollAndJam(self, ufactor, do_jamming, stmt, parallelize): '''To apply unroll-and-jam transformation''' debug('orio.module.loop.submodule.unrolljam.UnrollJam: starting unrollAndJam') # perform the unroll-and-jam transformation t = transformation.Transformation(ufactor, do_jamming, stmt, parallelize) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def mpiOverlap(self, protocol, msgsize, communication, stmt): '''To apply MPI overlap transformation''' debug('orio.module.loop.submodule.mpioverlap.MPIOverlap: starting mpiOverlap') # perform the MPI overlap transformation t = transformation.Transformation(protocol, msgsize, communication, stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def applyTransf(self, tiles, permuts, regtiles, ujams, scalarrep, boundrep, pragma, openmp, vector, arrcopy, cuda, stmt): '''To apply a sequence of transformations''' # perform the composite transformations t = transformation.Transformation(tiles, permuts, regtiles, ujams, scalarrep, boundrep, pragma, openmp, vector, arrcopy, cuda, self.stmt) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def cudify(self, stmt, targs): '''Apply CUDA transformations''' g.debug( 'orio.module.loop.submodule.cuda.CUDA: starting CUDA transformations' ) # perform transformation t = transformation.Transformation(stmt, self.props, targs, self.tinfo) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def openclify(self, stmt, targs): '''Apply OpenCL transformations''' g.debug( 'orio.module.loop.submodule.opencl.opencl: starting OpenCL transformations' ) # perform transformation t = transformation.Transformation(stmt, self.props, targs, self.tinfo) transformed_stmt = t.transform() # return the transformed statement return transformed_stmt
def transform(self): '''To apply loop transformations on the annotated code''' # parse the code to get the AST stmts = parser.getParser(self.line_no).parse(self.module_body_code) if isinstance(stmts[0], ast.TransformStmt) and stmts[0].stmt is None: # transform the enclosed annot_body_code annotated_stmts = parser.getParser(self.line_no).parse( self.annot_body_code) if len(annotated_stmts) == 1: annotated_stmt = annotated_stmts[0] else: annotated_stmt = ast.CompStmt(annotated_stmts[0]) stmts[0].stmt = annotated_stmt # apply transformations t = transformation.Transformation(self.perf_params, self.verbose, self.language, self.tinfo) transformed_stmts = t.transform(stmts) # generate code for the transformed ASTs indent = ' ' * self.indent_size extra_indent = ' ' cgen = codegen.CodeGen(self.language) transformed_code = '\n' for s in transformed_stmts: transformed_code += cgen.generate(s, indent, extra_indent) # Example on applying another visitor, e.g., for analysis #exampleVisitor = astvisitors.ExampleVisitor() #exampleVisitor.visit(transformed_stmts) # Count operations visitor opsVisitor = astvisitors.CountingVisitor() opsVisitor.visit(transformed_stmts) debug(str(opsVisitor), level=3) # CFG if True: try: from orio.module.loop.cfg import CFGGraph cfg = CFGGraph(transformed_stmts) except Exception, e: err('[module.loop.loop] cannot construct CFG: ', e)
def transform(self): '''To apply loop tiling on the annotated code''' # parse the text in the annotation orio.module.body to extract tiling information tiling_info = ann_parser.AnnParser(self.perf_params).parse(self.module_body_code) # parse the code (in the annotation body) to extract the corresponding AST stmts = code_parser.getParser().parse(self.annot_body_code) # analyze the AST semantics stmts = semant.SemanticAnalyzer(tiling_info).analyze(stmts) # perform loop-tiling transformation t = transformation.Transformation(tiling_info) (stmts, int_vars) = t.transform(stmts) # generate the tiled code code = self.__generateCode(stmts, int_vars) # return the tiled code return code
def transform(self): '''To perform loop tiling''' # parse the text in the annotation orio.module.body to extract the tiling information tiling_params = ann_parser.AnnParser(self.perf_params).parse( self.module_body_code) # parse the code (in the annotation body) to extract the corresponding AST stmts = code_parser.getParser().parse(self.annot_body_code) # check and enforce the AST semantics stmts = semant.SemanticChecker().check(stmts) # perform loop-tiling transformation (stmts, int_vars ) = transformation.Transformation(tiling_params).transform(stmts) # generate the tiled code code = self.__generate(stmts, int_vars) # return the tiled code return code
def transform(self): """To apply loop tiling on the annotated code""" # parse the text in the annotation orio.module.body to extract variable value pairs var_val_pairs = ann_parser.AnnParser(self.perf_params).parse( self.module_body_code ) # filter out some variables used for turning on/off the optimizations unroll = 1 vectorize = 1 scalar_replacement = 1 constant_folding = 1 tile_sizes = [] for var, val in var_val_pairs: if var == "unroll": unroll = val elif var == "vectorize": vectorize = val elif var == "scalar_replacement": scalar_replacement = val elif var == "constant_folding": constant_folding = val else: tile_sizes.append((var, val)) # remove all annotations from the annotation body text ann_re = r"/\*@\s*(.|\n)*?\s*@\*/" code = re.sub(ann_re, "", self.annot_body_code) # extract code regions that cover the full core tiles code_regions = self.__extractCoreTiles(code, tile_sizes) # parse the full core-tile code and generate a corresponding AST n_code_regions = [] for cr in code_regions: if isinstance(cr, str): n_code_regions.append(cr) continue i, v, c = cr stmts = code_parser.getParser().parse(c) if len(stmts) != 1 or not isinstance(stmts[0], ast.ForStmt): err("orio.module.ortildriver.ortildriver: invalid full core-tile code") n_code_regions.append((i, v, stmts[0])) code_regions = n_code_regions # transform the full core-tile code transformed_code = "" for cr in code_regions: if isinstance(cr, str): transformed_code += cr continue i, v, s = cr t = transformation.Transformation( unroll, vectorize, scalar_replacement, constant_folding ) transformed_code += t.transform(i, v, s) # insert the declaration code for the tile sizes decl_code = "" for i, (tvar, tval) in enumerate(tile_sizes): decl_code += " %s = %s;\n" % (tvar, tval) if transformed_code[0] != "\n": transformed_code = "\n" + transformed_code transformed_code = "\n" + decl_code + transformed_code # return the transformed code return transformed_code
# vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]]) # print(TM_test.TM) # THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left") # print(THETAS) # Get each vector of theta_i for all points THETA_1 = [] THETA_2 = [] THETA_4 = [] THETA_5 = [] THETA_6 = [] THETA_7 = [] for i in range(len(vector_x)): TM_i = transformation.Transformation( vector_angle_x[i], vector_angle_y[i], vector_angle_z[i], [vector_x[i], vector_y[i], vector_z[i]]) THETAS_i = inverse_kinematics.inverse_kinematics_baxter(TM_i.TM, "left") THETA_1.append(float(THETAS_i[0])) THETA_2.append(float(THETAS_i[1])) THETA_4.append(float(THETAS_i[2])) THETA_5.append(float(THETAS_i[3])) THETA_6.append(float(THETAS_i[4])) THETA_7.append(float(THETAS_i[5])) if SHOW_RESULTS == True: print("\n THETA_1: \n", np.rad2deg(THETA_1)) print("\n max(THETA_1) =", max(np.rad2deg(THETA_1))) print("\n min(THETA_1) =", min(np.rad2deg(THETA_1)))
def detection(self, high_contrast): tlc = TextLineColeection(self.img_data.textLines) corners = [] rows, cols = self.img_data.crop_gray.shape if(high_contrast): expandX = int (float(cols) * 0.5) expandY = int(float(rows) * 0.5) w = cols h = rows corners.append((-1 * expandX, -1 * expandY)) corners.append((expandX + w, -1 * expandY)) corners.append((expandX + w, expandY + h)) corners.append((-1 * expandX, expandY + h)) elif tlc.longerSegment.length > tlc.charHeight * 3: charHeightToPlateWidthRatio = 304.8 / 70 idealPixelWidth = tlc.charHeight * (charHeightToPlateWidthRatio * 1.03) charHeightToPlateHeightRatio = 152.4 / 70 idealPixelHeight = tlc.charHeight * charHeightToPlateHeightRatio verticalOffset = idealPixelHeight * 1.5 / 2 horizontaOffset = idealPixelWidth * 1.25 / 2 topLine = tlc.centerHorizontalLine.getParalleLine(verticalOffset) bottomLine = tlc.centerHorizontalLine.getParalleLine(-1 * verticalOffset) leftLine = tlc.centerVerticalLine.getParalleLine(-1 * horizontaOffset) rightLine = tlc.centerVerticalLine.getParalleLine(horizontaOffset) topLeft = topLine.intersection(leftLine) topRight = topLine.intersection(rightLine) botRight = bottomLine.intersection(rightLine) botLeft = bottomLine.intersection(leftLine) corners.append(topLeft) corners.append(topRight) corners.append(botRight) corners.append(botLeft) else: expandX = int(float(cols) * 0.15) expandY = int(float(rows) * 0.15) w = cols h = rows corners.append((-1 * expandX, -1 * expandY)) corners.append((expandX + w, -1 * expandY)) corners.append((expandX + w, expandY + h)) corners.append((-1 * expandX, expandY + h)) width = self.img_data.grayImg.shape[1] height = self.img_data.grayImg.shape[0] imgTransform = transformation.Transformation(self.img_data.grayImg, self.img_data.crop_gray, self.img_data.regionOfInterest.x, self.img_data.regionOfInterest.y, self.img_data.regionOfInterest.width, self.img_data.regionOfInterest.height) remappedCorners = imgTransform.transformSmallPointsTOBigImage(corners) cropSize = imgTransform.getCropSize(remappedCorners, (120, 60)) transmtx = imgTransform.getTransformationMatrix1(remappedCorners, cropSize[0], cropSize[1]) newCrop = imgTransform.crop(cropSize, transmtx) newLines = [] for i in range(0, len(self.img_data.textLines)): textArea = imgTransform.transformSmallPointsTOBigImage(self.img_data.textLines[i].textArea) linePolygon = imgTransform.transformSmallPointsTOBigImage(self.img_data.textLines[i].linePolygon) textAreaRemapped = imgTransform.remapSmallPointstoCrop(textArea, transmtx) linePolygonRemapped = imgTransform.remapSmallPointstoCrop(linePolygon, transmtx) newLines.append(textline.TextLine(textAreaRemapped[0], linePolygonRemapped[0], newCrop.shape[1], newCrop.shape[0])) smallPlateCorners = [] if high_contrast: smallPlateCorners = self.highContrastDetection(newCrop, newLines) else: smallPlateCorners = self.normalDetection(newCrop, newLines) imgArea = [] imgArea.append((0, 0)) imgArea.append((newCrop.shape[1], 0)) imgArea.append((newCrop.shape[1], newCrop.shape[0])) imgArea.append((0, newCrop.shape[0])) newCropTransmtx = imgTransform.getTransformationMatrix2(imgArea, remappedCorners) cornersInOriginalImg = [] if len(smallPlateCorners) > 0: cornersInOriginalImg = imgTransform.remapSmallPointstoCrop(smallPlateCorners, newCropTransmtx) return cornersInOriginalImg
def __init__(self): # Initialize main node for publishing rospy.init_node('MainAction') rospy.loginfo('... Initializing MainAction node') # Enable baxter if it isn't already enabled baxter_interface.RobotEnable(CHECK_VERSION).state().enabled baxter_interface.RobotEnable(CHECK_VERSION).enable() # Get create main publisher for activating the Joint movements self.pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) # Create Class to control Baxter's Joints self.command = JointCommand() self.rate = rospy.Rate(70) # Variable to show every print statement for testing SHOW_RESULTS = False # Define screen pixels for Baxter's image (face LCD screen) h = 600 w = 1024 # Load image for the Baxter's screen img1 = cv2.imread("Tejada.png") # Resize the image to get to the maximum possible screen size img1 = cv2.resize(img1, (w, h), interpolation=cv2.INTER_CUBIC) # Create publisher for the Image that will be loaded to Baxter pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) # Define message that will be loaded to the Image-publisher msg1 = cv_bridge.CvBridge().cv2_to_imgmsg(img1, encoding="bgr8") # Publish the desired message that contains the image pub.publish(msg1) # Get main vector for the trajectory T1 = trajectory_A.TrajectoryA(5) print("\n... Processing trajectory 1 ...\n") vector_x = T1.vector_x vector_y = T1.vector_y vector_z = T1.vector_z vector_angle_x = T1.vector_angle_x vector_angle_y = T1.vector_angle_y vector_angle_z = T1.vector_angle_z print("\n... Done processing trajectory 1 ...\n") # TM_test = transformation.Transformation(vector_angle_x[0], vector_angle_y[0], # vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]]) # print(TM_test.TM) # THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left") # print(THETAS) # Get each vector of theta_i for all points self.THETA_1 = [] self.THETA_2 = [] self.THETA_4 = [] self.THETA_5 = [] self.THETA_6 = [] self.THETA_7 = [] # Generate main trajectories finding each angle of the joints with IK for i in range(len(vector_x)): # Get each transformation matrix for all cartesian points given TM_i = transformation.Transformation( vector_angle_x[i], vector_angle_y[i], vector_angle_z[i], [vector_x[i], vector_y[i], vector_z[i]]) # Get each THETA_i for each point with the inverse-kinematics THETAS_i = inverse_kinematics.inverse_kinematics_baxter( TM_i.TM, "left") # Create all vectors of THETA_i for all necessary points self.THETA_1.append(float(THETAS_i[0])) self.THETA_2.append(float(THETAS_i[1])) self.THETA_4.append(float(THETAS_i[2])) self.THETA_5.append(float(THETAS_i[3])) self.THETA_6.append(float(THETAS_i[4])) self.THETA_7.append(float(THETAS_i[5])) if SHOW_RESULTS == True: print("\n THETA_1: \n", np.rad2deg(self.THETA_1)) print("\n max(THETA_1) =", max(np.rad2deg(self.THETA_1))) print("\n min(THETA_1) =", min(np.rad2deg(self.THETA_1))) print("\n THETA_2: \n", np.rad2deg(self.THETA_2)) print("\n max(THETA_2) =", max(np.rad2deg(self.THETA_2))) print("\n min(THETA_2) =", min(np.rad2deg(self.THETA_2))) print("\n THETA_4: \n", np.rad2deg(self.THETA_4)) print("\n max(THETA_4) =", max(np.rad2deg(self.THETA_4))) print("\n min(THETA_4) =", min(np.rad2deg(self.THETA_4))) print("\n THETA_5: \n", np.rad2deg(self.THETA_5)) print("\n max(THETA_5) =", max(np.rad2deg(self.THETA_5))) print("\n min(THETA_5) =", min(np.rad2deg(self.THETA_5))) print("\n THETA_6: \n", np.rad2deg(self.THETA_6)) print("\n max(THETA_6) =", max(np.rad2deg(self.THETA_6))) print("\n min(THETA_6) =", min(np.rad2deg(self.THETA_6))) print("\n THETA_7: \n", np.rad2deg(self.THETA_7)) print("\n max(THETA_7) =", max(np.rad2deg(self.THETA_7))) print("\n min(THETA_7) =", min(np.rad2deg(self.THETA_7)))
def gen_espiral(self): # Variable to show every print statement for testing SHOW_RESULTS =False # Get main vector for the trajectory T1 = trajectory_A.TrajectoryA(5) print("\n... Processing trajectory 1 ...\n") vector_x = T1.vector_x vector_y = T1.vector_y vector_z = T1.vector_z vector_angle_x = T1.vector_angle_x vector_angle_y = T1.vector_angle_y vector_angle_z = T1.vector_angle_z print("\n... Done processing trajectory 1 ...\n") # TM_test = transformation.Transformation(vector_angle_x[0], vector_angle_y[0], # vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]]) # print(TM_test.TM) # THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left") # print(THETAS) # Get each vector of theta_i for all points THETA_1 = [] THETA_2 = [] THETA_4 = [] THETA_5 = [] THETA_6 = [] THETA_7 = [] for i in range(len(vector_x)): TM_i = transformation.Transformation(vector_angle_x[i], vector_angle_y[i], vector_angle_z[i], [vector_x[i], vector_y[i], vector_z[i]]) THETAS_i = inverse_kinematics.inverse_kinematics_baxter(TM_i.TM, "left") THETA_1.append(float(THETAS_i[0])) THETA_2.append(float(THETAS_i[1])) THETA_4.append(float(THETAS_i[2])) THETA_5.append(float(THETAS_i[3])) THETA_6.append(float(THETAS_i[4])) THETA_7.append(float(THETAS_i[5])) izq0 = {'left_w0': THETA_5[0], 'left_w1': THETA_6[0], 'left_w2': THETA_7[0], 'left_e0': 0, 'left_e1': THETA_4[0], 'left_s0': THETA_1[0], 'left_s1': THETA_2[0]} self.limbi.move_to_joint_positions(izq0) #VP1 izq1 = {'left_w0': THETA_5[9], 'left_w1': THETA_6[9], 'left_w2': THETA_7[9], 'left_e0': 0, 'left_e1': THETA_4[9], 'left_s0': THETA_1[9], 'left_s1': THETA_2[9]} self.limbi.move_to_joint_positions(izq1) #VP2 izq2 = {'left_w0': THETA_5[19], 'left_w1': THETA_6[19], 'left_w2': THETA_7[19], 'left_e0': 0, 'left_e1': THETA_4[19], 'left_s0': THETA_1[19], 'left_s1': THETA_2[19]} self.limbi.move_to_joint_positions(izq2) #VP3 izq3 = {'left_w0': THETA_5[29], 'left_w1': THETA_6[29], 'left_w2': THETA_7[29], 'left_e0': 0, 'left_e1': THETA_4[29], 'left_s0': THETA_1[29], 'left_s1': THETA_2[29]} self.limbi.move_to_joint_positions(izq3) #ir al centro izq5 = {'left_w0': THETA_5[39], 'left_w1': THETA_6[39], 'left_w2': THETA_7[39], 'left_e0': 0, 'left_e1': THETA_4[39], 'left_s0': THETA_1[39], 'left_s1': THETA_2[39]} self.limbi.move_to_joint_positions(izq5) #preparacion izq6 = {'left_w0': THETA_5[49], 'left_w1': THETA_6[49], 'left_w2': THETA_7[49], 'left_e0': 0, 'left_e1': THETA_4[49], 'left_s0': THETA_1[49], 'left_s1': THETA_2[49]} self.limbi.move_to_joint_positions(izq6)