fc_lasercladding_wb/freecad/LaserCladdingWorkbench/kuka.py

import FreeCAD
import numpy as np
import math
import time
import Part
import re
import copy


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"""



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, filename):
        if not filename.endswith('.src'):
            filename = filename +'.src'
        srcfile = open(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("$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("$ANOUT[1] = %f ;  \n" % self.laser_power)
        if not self.simulation:
            srcfile.write("$OUT[%d] = TRUE ;  \n" % self.powder_out)
            srcfile.write("$OUT[%d] = TRUE ;  \n" % self.inert_gas_out)
        else:
            srcfile.write("$OUT[%d] = FALSE ;  \n" % self.powder_out)
            srcfile.write("$OUT[%d] = FALSE ;  \n" % self.inert_gas_out)

        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("$VEL.CP = %f ; m/s ; m/s \n" % self.vproc)
            srcfile.write(";- Turn on Laser\n")
            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\n" % self.use_laser_out)
                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")
        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\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\n" % self.use_laser_out)
        # end of subroutine
        srcfile.write("$OUT[%d] = FALSE\n" % self.use_laser_out)
        srcfile.write("$OUT[%d] = FALSE\n" % self.powder_out)
        srcfile.write("$OUT[%d] = FALSE\n" % self.inert_gas_out)
        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