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)
