def insert_z_probe(self): gz1 = config.SIZE_Z gz2 = -2 gz3 = 3 if config.Z_INVERT: gz1 *= -1 gz2 *= -1 gz3 *= -1 self.__add_action(action.MCUCmd("M996", sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz1), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz2), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz3), feed=config.PRECISE_FEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action(action.MCUCmd("M995", sender=self.table_sender))
def __init__(self, location=euclid.Vector3(0.0, 0.0, 0.0), group="default"): # if a list or a tuple is passed in, convert it to a Vector3 if type(location).__name__=='list' or type(location).__name__=='tuple': location = euclid.Vector3(location[0], location[1], location[2]) self.location = location self.group = group
def __fill_max_feed_01(self, actions): # set maximal feed0 and feed1 for i in range(len(actions)): action = actions[i][0] dir0 = actions[i][1]["dir0"] if i > 0: prevdir = actions[i - 1][1]["dir1"] prevmf = actions[i - 1][0].max_feed else: prevdir = euclid3.Vector3(0, 0, 0) prevmf = 0 action.max_feed0 = min( [self.__max_feed_jerk(prevdir, dir0), action.max_feed, prevmf]) dir1 = actions[i][1]["dir1"] if i < len(actions) - 1: nextdir = actions[i + 1][1]["dir0"] nextmf = actions[i + 1][0].max_feed else: nextdir = euclid3.Vector3(0, 0, 0) nextmf = 0 action.max_feed1 = min( [self.__max_feed_jerk(nextdir, dir1), action.max_feed, nextmf])
def __process_move(self, frame): self.table_state.process_frame(frame) pos = arguments.Positioning(frame) if pos is not None and self.table_state.modals.units is positioning.Configuration.UnitsGroup.inches: pos.as_inches() feed = arguments.Feed(frame) stop = arguments.ExactStop(frame) tool = arguments.Tool(frame) no_motion = False for cmd in frame.commands: if cmd.type == "G": if cmd.value == 74: self.program.insert_homing() self.table_state.pos.x = 0 self.table_state.pos.y = 0 self.table_state.pos.z = 0 self.program.insert_reset_coordinates( x=self.table_state.pos.x, y=self.table_state.pos.y, z=self.table_state.pos.z) elif cmd.value == 30: self.program.insert_z_probe() self.table_state.pos.z = 0 self.program.insert_reset_coordinates( z=self.table_state.pos.z) elif cmd.value == 92: # set offset registers self.__set_coordinates(x=pos.X, y=pos.Y, z=pos.Z) cs = self.table_state.modals.coord_system csstr = str(cs) offset = self.table_state.modals.offsets[cs] offset = euclid3.Vector3(offset.x, offset.y, offset.z) self.program.insert_coordinate_system_change(csstr, offset) no_motion = True elif cmd.value >= 53 and cmd.value <= 59: cs = self.table_state.modals.coord_system csstr = str(cs) offset = self.table_state.modals.offsets[cs] offset = euclid3.Vector3(offset.x, offset.y, offset.z) self.program.insert_coordinate_system_change(csstr, offset) elif cmd.type == "M": if cmd.value == 6 and tool.tool != None: self.program.insert_select_tool(tool.tool, self.tool_select_cb) self.table_state.select_tool(tool.tool) if feed.feed != None: self.__set_feed(feed.feed) if pos.is_moving and not no_motion: self.table_state.pos = self.program.insert_move( pos, self.table_state) #print("Pos = ", self.table_state.pos) if stop.exact_stop is True: self.program.insert_stop()
def __init__(self): # global coordinates self.pos = euclid3.Vector3(0, 0, 0) # modals self.modals = Configuration() self.mstack = []
def import_mesh(output_model, mesh_name, blender_object, use_vertex_groups=True): blender_mesh = blender_object.data log.info("Processing mesh: ", mesh_name) output_mesh = output_model.addMesh(mesh_name) for vertex in blender_mesh.vertices: group = "group" if use_vertex_groups and len(vertex.groups) == 1: group = blender_object.vertex_groups[vertex.groups[0].group].name position = euclid.Vector3(vertex.co.x, vertex.co.y, vertex.co.z) output_mesh.addVertex(position, group) for polygon in blender_mesh.polygons: uvlist = None if len(blender_mesh.uv_layers) > 0: uv_data = blender_mesh.uv_layers[0].data uvlist = [ uv_data[polygon.loop_start + i].uv for i in range(0, polygon.loop_total) ] # Here we need to specify normals per-polygon, as opposed # to per-vertex. normals = [ blender_mesh.vertices[vertex].normal.normalized() * 0.95 for vertex in polygon.vertices ] material = blender_mesh.materials[polygon.material_index].name smooth_shading = polygon.use_smooth output_mesh.addPolygon(polygon.vertices, uvlist, normals, material, smooth_shading)
def __init__(self, d): self.delta = euclid3.Vector3(d, 0, 0) self.feed = 600 self.feed0 = 0 self.feed1 = 0 self.is_moving = True self.exact_stop = False
def update_current_coordinate_system(self, cs, shift=None): self.coordinate_system = cs if cs == "G53" or shift is None: self.__local_shift = euclid3.Vector3() else: self.__local_shift = shift print("LOCAL SHIFT = ", shift) self.current_crds = self.global_crds - self.__local_shift
def __convert_hw(self, hw): hwc = euclid3.Vector3(hw["x"], hw["y"], hw["z"]) if config.X_INVERT: hwc.x = -hwc.x if config.Y_INVERT: hwc.y = -hwc.y if config.Z_INVERT: hwc.z = -hwc.z return hwc
def bounding_sphere(self): # returns the center of the object, and the magnitude of the furthest # point from that center. all_points = [] all_points.extend(self.vertices) midpoint = sum([point.location for point in all_points], euclid.Vector3()) / len(all_points) radius = max( abs( abs(point.location - midpoint) ) for point in all_points) return midpoint, radius
def read(self, filename): with open(filename) as fp: for line in fp.readlines(): self.process_command(self.remove_comments(line)) # ok, now we have the obj read in, convert it to a model object = model.Model() #add the materials to the model for k in self.materials.keys(): object.addMaterial(k, self.materials[k]["ambient"], self.materials[k]["specular"], self.materials[k]["diffuse"]) # First, add the vertecies for point in self.v: object.addVertex(euclid.Vector3(point['x'], point['y'], point['z'])) # for each polygon in the model, add the appropriate # data to the model for face in self.f: # build a list of vertecies for this face points = [] uvlist = [] normals = [] for point in face["points"]: # todo: make sure this data is valid! points.append(point['v']) # do we have a uvlist? uvlist = [] if (point['vt'] != None): uvlist.append(( self.vt[point['vt']]['x'], self.vt[point['vt']]['y'], )) if len(uvlist) == 0: uvlist = None #similarly, do we have a Normals list? if (point['vn'] != None): normals.append(( self.vn[point['vn']]['x'], self.vn[point['vn']]['y'], self.vn[point['vn']]['z'], )) if len(normals) == 0: normals = None #object.polygons.append(model.Model.Polygon(points, uvlist)) object.addPoly(points, uvlist, normals, face["material"]) # todo: vertex normals here return object
def WorkReset(self): print("RESET") act = system.TableReset(sender=self.table_sender) act.run() self.reset = True self.table_sender.reset() self.spindle_sender.stop() self.state = None self.builder = None if self.program is not self.user_program and self.program is not self.empty_program: self.program.dispose() self.program = self.empty_program self.hw_coordinates = euclid3.Vector3(0, 0, 0)
def addPolygon(self, vertex_list=None, uvlist=None, vertex_normals=None, material=None, smooth=True): face_normal = self.face_normal(vertex_list) #if this is a 2-point polygon, turn it into a triangle; this will draw #on hardware as a perfect line segment if abs(self.vertices[vertex_list[0]].location - self.vertices[vertex_list[1]].location) < 0.01: log.debug("Encountered LINE SEGMENT variant 1") vertex_list = [vertex_list[0], vertex_list[2], vertex_list[2]] vertex_normals = [vertex_normals[0], vertex_normals[2], vertex_normals[2]] face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2]) #pick one at random elif abs(self.vertices[vertex_list[1]].location - self.vertices[vertex_list[2]].location) < 0.01: log.debug("Encountered LINE SEGMENT variant 2") vertex_list = [vertex_list[0], vertex_list[1], vertex_list[1]] vertex_normals = [vertex_normals[0], vertex_normals[1], vertex_normals[1]] face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2]) elif abs(self.vertices[vertex_list[2]].location - self.vertices[vertex_list[0]].location) < 0.01: log.debug("Encountered LINE SEGMENT variant 3") vertex_list = [vertex_list[0], vertex_list[1], vertex_list[1]] vertex_normals = [vertex_normals[0], vertex_normals[1], vertex_normals[1]] face_normal = euclid.Vector3(vertex_normals[0][0],vertex_normals[0][1],vertex_normals[0][2]) self.polygons.append(Model.Polygon(vertex_list, uvlist, material, face_normal, vertex_normals, self, smooth))
def find_geometry(source, target, ccw, axis, **kwargs): d, h = HelixMovement.__get_d_h(target - source, axis) # Now we don't support 'h'! if abs(h) > 1e-4: raise Exception("Arc don't support 'h'!") if "r" in kwargs.keys(): radius = kwargs["r"] start_to_center, tan0, tan1, arc_angle = HelixMovement.__find_geometry_from_r( d, radius, ccw) else: if axis == HelixMovement.Axis.xy: start_to_center = euclid3.Vector2(kwargs["i"], kwargs["j"]) elif axis == HelixMovement.Axis.yz: start_to_center = euclid3.Vector2(kwargs["j"], kwargs["k"]) else: start_to_center = euclid3.Vector2(kwargs["k"], kwargs["i"]) radius, tan0, tan1, arc_angle = HelixMovement.__find_geometry_from_ijk( d, start_to_center, ccw) if axis == HelixMovement.Axis.xy: dir_0 = euclid3.Vector3(tan0.x, tan0.y, 0) dir_1 = euclid3.Vector3(tan1.x, tan1.y, 0) center = source + euclid3.Vector3(start_to_center[0], start_to_center[1], 0) elif axis == HelixMovement.Axis.yz: dir_0 = euclid3.Vector3(0, tan0.x, tan0.y) dir_1 = euclid3.Vector3(0, tan1.x, tan1.y) center = source + euclid3.Vector3(0, start_to_center[0], start_to_center[1]) elif axis == HelixMovement.Axis.zx: dir_0 = euclid3.Vector3(tan0.y, 0, tan0.x) dir_1 = euclid3.Vector3(tan1.y, 0, tan1.x) center = source + euclid3.Vector3(start_to_center[1], 0, start_to_center[0]) return center, dir_0, dir_1, arc_angle
def __init__(self, table_sender, spindle_sender): print("Creating machine...") self.registers = {"tools": {}} self.is_running = False self.is_finished = False self.table_sender = table_sender self.spindle_sender = spindle_sender self.running = event.EventEmitter() self.paused = event.EventEmitter() self.finished = event.EventEmitter() self.error = event.EventEmitter() self.line_selected = event.EventEmitter() self.tool_selected = event.EventEmitter() self.on_coordinates = event.EventEmitter() self.reset = False self.action_rdy = threading.Event() self.__table_reseted_ev = threading.Event() self.current_wait = None self.c_actions = [] self.nc_action = None self.opt = Optimizer(common.config.JERKING, common.config.ACCELERATION, common.config.MAXFEED) # loaded program self.user_program = None self.user_frames = [] # actual program self.program = None self.state = None self.builder = None # special programs self.empty_program = pr.Program(self.table_sender, self.spindle_sender) # states self.previous_state = None self.work_init(self.empty_program) self.table_sender.mcu_reseted += self.__table_reseted # current states self.runtime_state = runtimestate.RuntimeState({ "x": 0, "y": 0, "z": 0 }, euclid3.Vector3()) print("done")
def get_euler(self): q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3) euler = q.get_euler() return euclid3.Vector3(euler[2] * 57.29577951, euler[0] * 57.29577951, euler[1] * 57.29577951)
def get_gyro(self): return euclid3.Vector3(self.gx, self.gy, self.gz)
def get_acc(self): return euclid3.Vector3(self.ax, self.ay, self.az)
def addVertex(self, location=euclid.Vector3(0.0, 0.0, 0.0), group="default"): self.vertices.append(Model.Vertex(location, group)) if group not in self.model.groups: self.model.groups.append(group)
def insert_homing(self): gz1 = -config.SIZE_Z gz2 = 2 gz3 = -3 if config.Z_INVERT: gz1 *= -1 gz2 *= -1 gz3 *= -1 gx1 = -config.SIZE_X gx2 = 2 gx3 = -3 if config.X_INVERT: gx1 *= -1 gx2 *= -1 gx3 *= -1 gy1 = -config.SIZE_Y gy2 = 2 gy3 = -3 if config.Y_INVERT: gy1 *= -1 gy2 *= -1 gy3 *= -1 self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz1), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz2), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, 0, gz3), feed=config.PRECISE_FEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(gx1, 0, 0), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(gx2, 0, 0), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(gx3, 0, 0), feed=config.PRECISE_FEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, gy1, 0), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, gy2, 0), feed=config.MAXFEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action( linear.LinearMovement(delta=euclid3.Vector3(0, gy3, 0), feed=config.PRECISE_FEED, acc=config.ACCELERATION, sender=self.table_sender)) self.__add_action(action.MCUCmd("M998", sender=self.table_sender)) self.__add_action(action.MCUCmd("M997", sender=self.table_sender))
def insert_move(self, pos, table_state): print("*** Insert move ", pos.X, pos.Y, pos.Z) #traceback.print_stack() #print(table_state.positioning) if table_state.modals.positioning == positioning.Configuration.PositioningGroup.absolute: cs = table_state.modals.offsets[table_state.modals.coord_system] origpos = table_state.pos origpos = cs.global2local(origpos.x, origpos.y, origpos.z) newpos = origpos if pos.X != None: newpos = euclid3.Vector3(pos.X, newpos.y, newpos.z) if pos.Y != None: newpos = euclid3.Vector3(newpos.x, pos.Y, newpos.z) if pos.Z != None: newpos = euclid3.Vector3(newpos.x, newpos.y, pos.Z) delta = newpos - origpos else: delta = euclid3.Vector3() if pos.X != None: delta.x = pos.X if pos.Y != None: delta.y = pos.Y if pos.Z != None: delta.z = pos.Z source = table_state.pos target = source + delta offset = 0 # Normal linear move if table_state.modals.motion == positioning.Configuration.MotionGroup.line: self.__insert_payload_movement(source, target, offset, table_state) # Fast linear move elif table_state.modals.motion == positioning.Configuration.MotionGroup.fast_move: self.__insert_fast_movement(source, target, offset, table_state) # Arc movement elif table_state.modals.motion == positioning.Configuration.MotionGroup.round_cw or \ table_state.modals.motion == positioning.Configuration.MotionGroup.round_ccw: if pos.R != None: self.__insert_arc_R(source, target, offset, pos.R, table_state) else: ci = 0 cj = 0 ck = 0 if pos.I != None: ci = pos.I if pos.J != None: cj = pos.J if pos.K != None: ck = pos.K self.__insert_arc_IJK(source, target, offset, ci, cj, ck, table_state) else: raise Exception("Not implemented %s motion state" % table_state.modals.motion) return target
def update_global_coordinates(self, hw, glob): self.hw_crds = euclid3.Vector3(hw["x"], hw["y"], hw["z"]) self.__hwc_crds = self.__convert_hw(hw) self.global_crds = glob self.__shift = self.global_crds - self.__hwc_crds
def global2local(self, x, y, z): return euclid3.Vector3(x - self.x, y - self.y, z - self.z)
def update_coordinates(self, hw): self.hw_crds = euclid3.Vector3(hw["x"], hw["y"], hw["z"]) self.__hwc_crds = self.__convert_hw(hw) self.global_crds = self.__hwc_crds + self.__shift self.current_crds = self.global_crds - self.__local_shift
def get_acc_earth(self): q = euclid3.Quaternion(self.q0, self.q1, self.q2, self.q3) a = euclid3.Vector3(self.ax, self.ay, self.az) e = q * a return euclid3.Vector3(e.x, e.y, e.z)
def __init__(self, hw, glob): self.coordinate_system = "G53" self.__local_shift = euclid3.Vector3() self.update_global_coordinates(hw, glob)
def local2global(self, x, y, z): return euclid3.Vector3(x + self.x, y + self.y, z + self.z)
def process_mesh(self, object, mesh): # Import polygon and vertex data. log.debug("Polygons: %d", mesh.GetPolygonCount()) log.debug("Verticies: %d", len(mesh.GetControlPoints())) #this list contains all the points in the model; polygons will #index into this list vertex_list = mesh.GetControlPoints() #add the verticies to the model for i in range(len(vertex_list)): object.addVertex( euclid.Vector3(vertex_list[i][0], vertex_list[i][1], vertex_list[i][2])) #do something about materials self.process_materials(object, mesh) material_map = mesh.GetLayer(0).GetMaterials().GetIndexArray() log.debug("Mesh Global Transform:") log.debug(fbx_to_euclid(mesh.GetNode().EvaluateGlobalTransform())) #exit() #well ... that explains a lot. self.mesh_global = fbx_to_euclid( mesh.GetNode().EvaluateGlobalTransform()) object.global_matrix = self.mesh_global for face in range(mesh.GetPolygonCount()): #this importer only supports triangles and #quads, so we need to throw out any weird #sizes here vertex_count = mesh.GetPolygonSize(face) if vertex_count >= 3 and vertex_count <= 4: points = [] normals = [] uvlist = [] for v in range(vertex_count): points.append(mesh.GetPolygonVertex(face, v)) #figure out if there's normal data? #TODO: Why does this need to loop over layer data? Investigate! for l in range(mesh.GetLayerCount()): normal_data = mesh.GetLayer(l).GetNormals() if normal_data: normal = normal_data.GetDirectArray().GetAt(v) #print(normal) normals.append((normal[0], normal[1], normal[2])) uv_data = mesh.GetLayer(l).GetUVs() if uv_data: if uv_data.GetMappingMode( ) == FbxLayerElement.eByControlPoint: log.warn( "eByControlPoint not supported for UVs!") elif uv_data.GetMappingMode( ) == FbxLayerElement.eByPolygonVertex: uv_index = mesh.GetTextureUVIndex(face, v) uv = uv_data.GetDirectArray().GetAt(uv_index) #print("UVs: ", uv) uvlist.append((uv[0], uv[1])) #todo: not discard UV coordinates here if len(uvlist) == 0: uvlist = None object.addPoly( points, uvlist, normals, mesh.GetNode().GetMaterial( material_map.GetAt(face)).GetName()) self.process_clusters(object, mesh)