jkur
/
fc_lasercladding_wb
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
5 Commits
1 Branch
0 Tags
683 KiB
 
Tree: f3bff01430
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'f3bff01430'
${ noResults }
fc_lasercladding_wb/freecad/LaserCladdingWorkbench/kuka.py

309 lines
12 KiB
Raw Blame History

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