|
|
import FreeCAD |
|
|
import numpy as np |
|
|
import math |
|
|
import time |
|
|
import Part |
|
|
|
|
|
|
|
|
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 = * |
|
|
""" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class Kuka_Prog: |
|
|
def __init__(self): |
|
|
self.pose_list = [] |
|
|
self.base = (0,0,0) |
|
|
|
|
|
def append_pose(self, pose): |
|
|
self.pose_list.append(pose) |
|
|
|
|
|
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): |
|
|
path = Part.makePolygon([FreeCAD.Base.Vector(p.X, p.Y, p.Z) for p in self.pose_list ]) |
|
|
s = Part.show(path) |
|
|
s.ViewObject.LineColor=(1.0,0.5,0.0) |
|
|
s.ViewObject.LineWidth=(2.5) |
|
|
|
|
|
def save_prog(self, filename): |
|
|
srcfile = open(filename+'.src','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(";------------- 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("$BASE = BASE_DATA[6] ; SET BASE to WORKPLACE\n\n") |
|
|
srcfile.write("\n;------------- main part ------------\n") |
|
|
|
|
|
#V = w.Velocity / 1000.0 # from mm/s to m/s |
|
|
V = 0.05 |
|
|
srcfile.write("$VEL.CP = %f ; m/s ; m/s \n"%V) |
|
|
for pose in self.pose_list: |
|
|
srcfile.write("LIN {} ; GENERATED\n".format(pose.to_string())) |
|
|
# end of subroutine |
|
|
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): |
|
|
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) + "}" |
|
|
|
|
|
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, edge, n): |
|
|
poses = [] |
|
|
points = edge.discretize(n) |
|
|
lines = [] |
|
|
for point in points: |
|
|
uv = face.Surface.parameter(point) |
|
|
normal = face.normalAt(uv[0], uv[1]) |
|
|
line=Part.makeLine(point, point+3*normal) |
|
|
lines.append(line) |
|
|
# create pose |
|
|
pose = Kuka_Pose.from_point_and_normal(point, normal) |
|
|
poses.append(pose) |
|
|
return poses
|
|
|
|
