import FreeCAD import math import time import Part import copy import os TeachPointFold = """ ;FOLD LIN P4 Vel= 0.2 m/s CPDAT1 Tool[1] Base[0];%{PE}%R 5.4.27,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P4, 3:, 5:0.2, 7:CPDAT1 $BWDSTART = FALSE LDAT_ACT=LCPDAT1 FDAT_ACT=FP4 BAS(#CP_PARAMS,0.2) LIN XP4 ;ENDFOLD """ TeachPointDat = """ DECL E6POS XP4={X -25.1844196,Y 1122.42603,Z 1158.07996,A -14.3267002,B 0.537901878,C 179.028305,S 6,T 59,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP4={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "} DECL LDAT LCPDAT1={VEL 2.0,ACC 100.0,APO_DIST 100.0,APO_FAC 50.0,ORI_TYP #VAR} """ header_src = """&ACCESS RVP &REL 1 &PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe &PARAM EDITMASK = * """ ptp_fold = """;FOLD PTP xp1 Vel=100 % PDAT1 Tool[6]:laser6 Base[2]:Laser;%{PE}%R 8.2.24,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:xp1, 3:, 5:100, 7:PDAT1 $BWDSTART=FALSE PDAT_ACT=PPDAT1 FDAT_ACT=Fxp1 BAS(#PTP_PARAMS,100) PTP Xxp1 ;ENDFOLD""" z_up_pose = """{X 0.0, Y 0.0, Z 50.0, A 0.0, B 0.0, C 0.0, E1 0.0, E2 0.0}""" class Kuka_Prog: def __init__(self): self.contour_path_list = [] self.hatchlines_list = [] self.baseorigin = (0, 0, 0) self.tool = 6 self.base = 6 self.vproc = 0.023 self.vmax = 0.15 self.laser_power = 0.4 self.laser_out = 3 self.laser_pilot_out = 4 # default is pilot laser self.use_laser_out = self.laser_pilot_out self.inert_gas_out = 9 self.powder_out = 7 self.simulation = True def set_baseorigin(self, vec): self.baseorigin = (vec.x, vec.y, vec.z) def set_tool(self, tool): self.tool = tool def set_base(self, base): self.base = base def set_velocity(self, vproc, vmax): self.vproc = vproc self.vmax = vmax def set_laser_power(self, power): self.laser_power = power def set_laser_out(self, laser_output): self.laser_out = laser_output def set_laser_pilot_out(self, laser_pilot_out): self.laser_pilot_out = laser_pilot_out def set_simulation(self, sim): self.simulation = sim if not self.simulation: self.use_laser_out = self.laser_out else: self.use_laser_out = self.laser_pilot_out def append_contour(self, poses, segmenttype='LIN'): self.contour_path_list.append((poses, segmenttype)) def append_hatchline(self, line, segmenttype='LIN'): if not len(self.hatchlines_list): self.hatchlines_list.append((line, segmenttype)) # poses are sorted # but maybe we need to reverse # get the point distance from first and last pose last, _ = self.hatchlines_list[-1] nfirst = line[0] nlast = line[-1] last = FreeCAD.Base.Vector(last[1].X, last[1].Y, last[1].Z) nlast = FreeCAD.Base.Vector(nlast.X, nlast.Y, nlast.Z) nfirst = FreeCAD.Base.Vector(nfirst.X, nfirst.Y, nfirst.Z) dnl = last.distanceToPoint(nlast) dnf = last.distanceToPoint(nfirst) if dnl < dnf: line.reverse() self.hatchlines_list.append((line, segmenttype)) def append_poses(self, poses): if not len(self.pose_list): self.pose_list.extend(poses) # poses are sorted # but maybe we need to reverse # get the point distance from first and last pose last = self.pose_list[-1] nfirst = poses[0] nlast = poses[-1] last = FreeCAD.Base.Vector(last.X, last.Y, last.Z) nlast = FreeCAD.Base.Vector(nlast.X, nlast.Y, nlast.Z) nfirst = FreeCAD.Base.Vector(nfirst.X, nfirst.Y, nfirst.Z) dnl = last.distanceToPoint(nlast) dnf = last.distanceToPoint(nfirst) if dnl < dnf: poses.reverse() self.pose_list.extend(poses) def draw_wire(self, obj): path = Part.makePolygon([FreeCAD.Base.Vector(p.X, p.Y, p.Z) for p in self.pose_list ]) #s = Part.show(path) obj.addObject(s) s.ViewObject.LineColor=(1.0,0.5,0.0) s.ViewObject.LineWidth=(2.5) def get_vectors(self): return [FreeCAD.Base.Vector(p.X, p.Y, p.Z) for p in poses for poses in self.contour_path_list ] def save_prog(self, article, path): if self.simulation: filename = "kvt_{}_sim.src".format(article) else: filename = "kvt_{}.src".format(article) srcfile = open(os.path.join(path, filename), 'w') srcfile.write(header_src) # subroutine definition srcfile.write("DEF "+filename+"( )\n\n") srcfile.write(";- Kuka src file, generated by KVT\n") srcfile.write(";- " + time.asctime() + "\n\n") # defining world and base srcfile.write("E6POS startp\n") srcfile.write("E6POS point1\n") # srcfile.write("DECL E6AXIS xp1={A1 -1.9, A2 -105.76, A3 79.97, A4 178.83, A5 -20.3, A6 -4.37, E1 -90, E2 0}\n") srcfile.write(";------------- definitions ------------\n") srcfile.write("EXT BAS (BAS_COMMAND :IN,REAL :IN ) ;set base to World\n") srcfile.write("BAS (#INITMOV,0 ) ;Initialicing the defaults for Vel and so on \n\n") srcfile.write("BAS (#TOOL,%d) ;Initialicing the defaults for Vel and so on \n\n" % self.tool) srcfile.write("BAS (#BASE,%d) ;Initialicing the defaults for Vel and so on \n\n" % self.base) srcfile.write("PTP {A1 -33.31, A2 -104.71, A3 114.60, A4 282.66, A5 -39.21, A6 -104.87, E1 -90, E2 1.0}\n") srcfile.write("\n;------------- main part ------------\n") srcfile.write("startp=$POS_ACT\n") #V = w.Velocity / 1000.0 # from mm/s to m/s CDIS = 2.3 CVEL = 95.0 srcfile.write(";- Movement paramters\n") srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax) srcfile.write("$APO.CDIS = %f ; mm \n" % CDIS) srcfile.write("$APO.CVEL = %f ; percent \n" % CVEL) srcfile.write(";- Input/Output settings\n") srcfile.write("$ANOUT[1] = %f ; Set laser power\n" % self.laser_power) srcfile.write("$OUT[%d] = FALSE ; Set Laser off\n" % self.use_laser_out) # don't know but must be on if not self.simulation: srcfile.write("$OUT[2] = TRUE ; Set Laser activation on\n") # don't know but must be on srcfile.write("$OUT[%d] = TRUE ; Set powder on \n" % self.powder_out) srcfile.write("$OUT[%d] = TRUE ; Set inert gas on \n" % self.inert_gas_out) else: srcfile.write("$OUT[%d] = FALSE ; set powder off \n" % self.powder_out) srcfile.write("$OUT[%d] = FALSE ; set inert gas off\n" % self.inert_gas_out) srcfile.write(";- Starting point\n") srcfile.write("point1 = {X -110.0, Y 0.0, Z 0.0, A 0.0000, B 0.0000, C 0.0000, E1 0.0000, E2 0.0000}\n") srcfile.write("point1.S = startp.S\n") srcfile.write("point1.T = startp.T\n") srcfile.write("LIN point1 C_VEL; GENERATED\n") srcfile.write("WAIT SEC 7.0\n") srcfile.write(";- Contourpaths\n") for (poses, seg_type) in self.contour_path_list: # start laser code srcfile.write(";- Move to pose up from contour start\n") srcfile.write("$VEL.CP = %f ; m/s ; m/s (vmax)\n" % self.vmax) srcfile.write("LIN_REL {Z 90.0} C_VEL; relative 90mm up\n") srcfile.write("LIN {}:{} C_VEL; move to start point but save z distance\n".format(poses[0].translate_with(self.baseorigin).to_string(), z_up_pose)) if seg_type == 'LIN': srcfile.write("LIN {} C_VEL; GENERATED\n".format(poses[0].translate_with(self.baseorigin).to_string())) srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=True ; Turn on Laser at point\n" % self.use_laser_out) srcfile.write("$VEL.CP = %f ; m/s ; m/s (vproc)\n" % self.vproc) for pose in poses[1:]: srcfile.write("LIN {} C_VEL; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string())) if seg_type == 'SPLINE': srcfile.write("SPLINE\n") for pose in poses: srcfile.write(" SPL {} ; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string())) srcfile.write("ENDSPLINE\n") srcfile.write(";- Turn off Laser\n") srcfile.write("$OUT[%d] = FALSE\n" % self.use_laser_out) # end of subroutine srcfile.write(";- Hatchlines\n") srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax) srcfile.write("LIN_REL {Z 90.0} C_VEL; just move up \n") (line, seg_type) = self.hatchlines_list[0] srcfile.write("LIN {}:{} C_VEL; move to first hatch point but with z_up\n".format(line[0].translate_with(self.baseorigin).to_string(), z_up_pose)) for (line, seg_type) in self.hatchlines_list: # start laser code srcfile.write(";- Hatchline\n") if seg_type == 'LIN': srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax) srcfile.write("LIN {} C_VEL; GENERATED\n".format(line[0].translate_with(self.baseorigin).to_string())) srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=True; Turn on Laser at point \n" % self.use_laser_out) srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vproc) srcfile.write("LIN {} C_VEL; GENERATED\n".format(line[1].translate_with(self.baseorigin).to_string())) srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=FALSE; Turn off Laser at point\n" % self.use_laser_out) # end of subroutine srcfile.write("$OUT[%d] = FALSE; laser off\n" % self.use_laser_out) srcfile.write("$OUT[%d] = FALSE; powder off\n" % self.powder_out) srcfile.write("$OUT[%d] = FALSE; inert gas off\n" % self.inert_gas_out) srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax) srcfile.write(";- Move to HOME position\n") srcfile.write("PTP {A1 -33.31, A2 -104.71, A3 114.60, A4 282.66, A5 -39.21, A6 -104.87, E1 -90, E2 1.0}\n") srcfile.write("\n;------------- end ------------\n") srcfile.write("END \n\n") srcfile.close() class Kuka_Pose: def __init__(self): self.X = 0.0 self.Y = 0.0 self.Z = 0.0 self.A = 0.0 self.B = 0.0 self.C = 0.0 self.S = 0 self.T = 0 self.E1 = 0.0 self.E2 = 0.0 def set_from_point_and_normal(self, point, normal): self.X = point.x self.Y = point.y self.Z = point.z r = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), normal) ABC_in_deg = r.toEulerAngles('ZYX') self.A = math.radians(ABC_in_deg[0]) self.B = math.radians(ABC_in_deg[1]) self.C = math.radians(ABC_in_deg[2]) #print("Rotation:", self.A, self.B, self.C) def from_point_and_normal(point, normal): pose = Kuka_Pose() pose.X = point.x pose.Y = point.y pose.Z = point.z r = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), normal) ABC_in_deg = r.toEulerAngles('ZYX') pose.A = math.radians(ABC_in_deg[0]) pose.B = math.radians(ABC_in_deg[1]) pose.C = math.radians(ABC_in_deg[2]) #print("Rotation:", self.A, self.B, self.C) return pose def to_string(self, rot=False): if rot: pose_string="X {:.3f}, Y {:.3f}, Z {:.3f}, A {:.4f}, B {:.4f}, C {:.4f}, E1 {:.4f}, E2 {:.4f}" return "{" + pose_string.format(self.X, self.Y, self.Z, self.A, self.B, self.C, self.E1, self.E2) + "}" else: pose_string="X {:.3f}, Y {:.3f}, Z {:.3f}, A {:.4f}, B {:.4f}, C {:.4f}, E1 {:.4f}, E2 {:.4f}" return "{" + pose_string.format(self.X, self.Y, self.Z, 0,0,0,0,0) + "}" def translate_with(self, vector): pose = copy.copy(self) pose.X = pose.X - vector[0] pose.Y = pose.Y - vector[1] pose.Z = pose.Z - vector[2] return pose def draw_pose(self): #line=Part.makeLine(point, point+3*normal) #lines.append(line) # from euler to some line # create upfacing vector then rotate around each axis? up = FreeCAD.Base.Vector(0,0,1) rotx = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(1,0,0), math.degrees(self.C)) roty = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,1,0), math.degrees(self.B)) rotz = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), math.degrees(self.A)) rot = rotz.multiply(roty.multiply(rotx)) up_rotated = rot.multVec(up) basepoint = FreeCAD.Base.Vector(self.X, self.Y, self.Z) line = Part.makeLine(basepoint, basepoint+5*up_rotated) #line.Placement.Rotation = rot s = Part.show(line) s.ViewObject.LineColor=(1.0,0.0,0.0) def get_list_of_poses(face, edges): poses = [] for edge in edges: p0 = edge.Vertexes[0].Point p1 = edge.Vertexes[1].Point for p in [p0, p1]: uv = face.Surface.parameter(p) normal = face.normalAt(uv[0], uv[1]) pose = Kuka_Pose.from_point_and_normal(p, normal) poses.append(pose) return poses