diff --git a/SVG_Block.py b/SVG_Block.py index a94aba1..d7b98e6 100644 --- a/SVG_Block.py +++ b/SVG_Block.py @@ -39,7 +39,7 @@ def load(self): # loads an svg file, puts the movements into self.path_movements # translates color and opacity values into the corresponding transformations start_time = time.time() - parsed_file = svg_parser.SVGParse(self.filepath, self.tolerance) + parsed_file = svg_parser.SVGParse(self.filepath, self.tolerance, self.scale) movements = parsed_file.convert_to_movements() self.path_movements = movements diff --git a/Toolpath.py b/Toolpath.py index 31b07d2..b6ab8b9 100644 --- a/Toolpath.py +++ b/Toolpath.py @@ -52,7 +52,7 @@ def __init__(self, coordinates, tool_length): # ************************************************** FORWARD KINEMATICS - def select(self,q_sols, q_d, w=[1, 1.5, 1.5, 1.5, 0.5, 0.5]): + def select(self,q_sols, q_d, w=[3, 1.5, 10, 5, 0.5, 0.5]): """Select the optimal solutions among a set of feasible joint value solutions. Args: @@ -175,12 +175,12 @@ def invKine(self, desired_pos, start_pos): # T60 T_34 = T_32 * T_21 * T_14 th[3, c] = atan2(T_34[1, 0], T_34[0, 0]) th = th.real - print("___" * 30) - print(th) + #print("___" * 30) + #print(th) th = np.transpose(th) th = th.tolist() - print(th) - print("___"*30) + #print(th) + #print("___"*30) best_th = th best_th = self.select(th, start_pos) @@ -189,6 +189,7 @@ def invKine(self, desired_pos, start_pos): # T60 # Calculates Rotation Matrix given euler angles. def eulerAnglesToRotationMatrix(self, theta): + print("euler angle calculation ") R_x = np.array([[1, 0, 0], [0, math.cos(theta[0]), -math.sin(theta[0])], [0, math.sin(theta[0]), math.cos(theta[0])] diff --git a/UR_Playground_0.8.1.py b/UR_Playground_0.8.1.py index 4a1cb77..3d26030 100644 --- a/UR_Playground_0.8.1.py +++ b/UR_Playground_0.8.1.py @@ -24,7 +24,7 @@ class BlockViewer(): logging.basicConfig(filename= "UR_Playground.log", level=logging.DEBUG, format = "%(asctime)s:%(levelname)s:%(message)s") logging.info("___"*45) -logging.info("UR_PLAYGROUND") +logging.info("UR_PLAYGROUND_0.8.1") class URPlayground(QMainWindow): @@ -41,7 +41,7 @@ def __init__(self, *args): self.svgblock = svgBlock() self.toolpath = None - self.setWindowTitle("UR_Playground") + self.setWindowTitle("UR_Playground_0.8.1") self.initialise_blocks() #self.make_log() @@ -91,11 +91,15 @@ def check_toolpath(self): # FIXME ... this is not done well at all fix this asa toolpath = Toolpath(self.svgblock.coordinates_travel[0], self.program.tcp[2]) coordinates = copy.copy(self.svgblock.coordinates_travel) previous_pose = self.objRB.q + csys = m3d.Transform(self.svgblock.csys) + tcp = m3d.Transform(self.program.tcp) + print("tcp and csys:") + print(tcp) + print(csys) + print("_____" * 25) for i, path in enumerate(coordinates): for coord in path: #convert coordinate to world coordinate system - csys = m3d.Transform(self.svgblock.csys) - tcp = m3d.Transform(self.program.tcp) t = csys * m3d.Transform(coord) t = t * tcp @@ -110,12 +114,12 @@ def check_toolpath(self): # FIXME ... this is not done well at all fix this asa desired_pose = rot_mat p_column = np.array([[pose[0]/1000],[pose[1]/1000],[pose[2]/1000]]) - print(p_column) + #print(p_column) full_matrix = np.append(rot_mat, p_column,axis=1) full_matrix = np.append(full_matrix, [[0,0,0,1]],axis = 0) - print(full_matrix) - print(coord) + #print(full_matrix) + #print(coord) best_jpose = toolpath.invKine(full_matrix, previous_pose) #self.objRB.JVars = best_jpose[:,1] @@ -125,6 +129,7 @@ def check_toolpath(self): # FIXME ... this is not done well at all fix this asa self.toolpath = toolpath.poses print("tp ", self.toolpath) #return toolpath.poses + del toolpath #self.objRB.JVars = best_jpose #self.RB.update() @@ -137,15 +142,18 @@ def play_toolpath(self): self.objRB.JVars = self.toolpath[int(tp_position)] self.RB.update() time.sleep(0.2) - else: - logging.info("No Toolpath. click check first") + #else: + #logging.info("No Toolpath. click check first") def make_toolpath(self): - self.toolpath = None - self.check_toolpath() - tp_position = len(self.toolpath) * self.ui.sliderPlay.sliderPosition()/100 - self.objRB.JVars = self.toolpath[int(tp_position)] - self.RB.update() + if len(self.svgblock.coordinates_travel) >0: + self.toolpath = None + self.check_toolpath() + tp_position = len(self.toolpath) * self.ui.sliderPlay.sliderPosition()/100 + self.objRB.JVars = self.toolpath[int(tp_position)] + self.RB.update() + else: + logging.info("No Toolpath. Load SVG File first") def update_toolpath_position(self): #print("slider position: ", self.ui.sliderPlay.sliderPosition()) diff --git a/svg_parser.py b/svg_parser.py index 45a96fe..fb7c393 100644 --- a/svg_parser.py +++ b/svg_parser.py @@ -14,7 +14,8 @@ class SVGParse: returns list of movement objects """ - def __init__(self, filepath, tolerance): + def __init__(self, filepath, tolerance, scale): + self.scale = scale self.path = filepath self.tol = tolerance self.movements = [] @@ -24,7 +25,7 @@ def convert_to_movements(self): paths, attributes = svgpathtools.svg2paths(self.path) for p_index, p in enumerate(paths): - print("----"*45) + #print("----"*45) p_attributes = attributes[p_index] # current path's attributes @@ -38,7 +39,7 @@ def interpolate_path(self,path,attributes): movement = MoveColOpa() # each path has one movement object length = path.length(error = self.tol) # TODO: if tolerance is very high amount = 0 and div throws an error - amount = int(length / self.tol) + amount = int(length / self.tol * self.scale) if amount == 0: amount = 2 div = 1 / amount @@ -106,13 +107,13 @@ def get_color(self,attributes): # check if there is a gradient if "url" in p_stroke: grad_bool = True - print("gradient in p") + #print("gradient in p") gradient_id = p_stroke[p_stroke.find("(") + 1: p_stroke.find(")")] gradient_id = gradient_id.strip("#") else: grad_bool = False - print("no gradient") + #print("no gradient") if grad_bool: return "gradient",gradient_id @@ -124,26 +125,27 @@ def get_color(self,attributes): def color_at_param(self,gradient, param): print("---"*25) - print("looking for color at param ") + #print("looking for color at param ") color = [] stops = gradient.stop_offsets print("stops: ", stops) clrs = gradient.colors for idx, offset in enumerate(stops) : - print("parsing all offsets.... current offset: ", offset, "at index: ", idx, "of: ", len(stops)-1) - print("offset: ", offset) - print("param: ", param) - print(len(gradient.opacities)) - print(len(clrs)) - print(len(stops)) + #print("parsing all offsets.... current offset: ", offset, "at index: ", idx, "of: ", len(stops)-1) + #print("offset: ", offset) + #print("param: ", param) + #print(len(gradient.opacities)) + #print(len(clrs)) + #print(len(stops)) if idx < (len(stops) - 1): - print("next offset: ", stops[idx + 1]) + #print("next offset: ", stops[idx + 1]) + pass # TODO:add case if param is smaller than first stop if param == offset : - print("param = offset") + #print("param = offset") return clrs[idx] elif param >= offset and param >= stops[idx + 1]: continue @@ -152,7 +154,7 @@ def color_at_param(self,gradient, param): para_1 = offset para_2 = stops[idx +1] position = (param - para_1) / (para_2 - para_1) - print("pos: ", position) + #print("pos: ", position) col_1 = clrs[idx] col_2 = clrs[idx + 1] @@ -161,14 +163,14 @@ def color_at_param(self,gradient, param): startval = rgb_val endval = col_2[val] interm_val = ((endval - startval) * position) + startval - print(interm_val) + #print(interm_val) color.append(interm_val) - print("param", param) - print("colour at param: ", color) + #print("param", param) + #print("colour at param: ", color) return color elif param == stops[idx + 1]: - print("param = next offset") + #print("param = next offset") return clrs[idx + 1] def opacities_on_path(self, path, attributes, stepsize, amount): @@ -192,7 +194,7 @@ def opacities_on_path(self, path, attributes, stepsize, amount): opacities.append(opacity) j += stepsize else: - print("applying opacitiesthis many times: ", amount) + print("applying opacities this many times: ", amount) # TODO: find out why you need to add 1 to the amount opacities = [1] * (amount+1) @@ -210,21 +212,22 @@ def get_path_opacity(self,path,attributes): # get the overall opacity of a path return opacity def opacity_at_param(self,gradient, param): - print("---" * 19) - print("looking for opacity at param ") + #print("---" * 19) + #print("looking for opacity at param ") stops = gradient.stop_offsets opas = gradient.opacities for idx, offset in enumerate(stops): - print("parsing all offsets.... current offset: ", offset, "at index: ", idx, "of: ", len(stops) - 1) - print("offset: ", offset) - print("param: ", param) + #print("parsing all offsets.... current offset: ", offset, "at index: ", idx, "of: ", len(stops) - 1) + #print("offset: ", offset) + #print("param: ", param) if idx < (len(stops) - 1): - print("next offset: ", stops[idx + 1]) + #print("next offset: ", stops[idx + 1]) + pass if param == offset: - print("param = offset") + #print("param = offset") return opas[idx] elif param >= offset and param >= stops[idx + 1]: continue @@ -232,22 +235,22 @@ def opacity_at_param(self,gradient, param): para_1 = offset para_2 = stops[idx + 1] position = (param - para_1) / (para_2 - para_1) - print("pos: ", position) + #print("pos: ", position) opa_1 = opas[idx] # this is one float opa_2 = opas[idx + 1] - print(type(opa_1)) - print(type(opa_2)) + #print(type(opa_1)) + #print(type(opa_2)) opacity = ((opa_2 - opa_1) * position) + opa_1 - print(type(opacity)) - print("current param opacity: ", opacity) + #print(type(opacity)) + # print("current param opacity: ", opacity) return opacity elif param == stops[idx + 1]: - print("param = next offset") + #print("param = next offset") return opas[idx + 1] def get_gradients(self): @@ -260,7 +263,7 @@ def get_gradients(self): if len(lin_gradients) > 0: for g in lin_gradients: - print("g:", g) + #print("g:", g) id = g.attributes["id"].value grad = Gradient(id)