def setFixedOrientations(routine,usemarks):
previousroll=0.0
for s in routine.Statements:
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
roll=math.degrees(math.atan2(m.A.Y,m.A.X))
pitch=math.degrees(math.atan2(math.sqrt(m.A.X**2+m.A.Y**2),m.A.Z))
if roll<=0.0:
roll2=roll+180.0
else:
roll2=roll-180.0
if abs(roll-previousroll)>abs(roll2-previousroll):
roll=roll2
pitch=-pitch
previousroll=roll
tmpmtx = vcMatrix.new()
tmpmtx.rotateRelZ(roll)
tmpmtx.rotateRelY(pitch)
m.N=tmpmtx.N
m.O=tmpmtx.O
m.A=tmpmtx.A
s.Target = m
def WT__createpattern(prodname, x_amount, y_amount, z_amount, x_step, y_step, z_step, start_range, end_range):
global in_connector
ccomp = compsInWorld.get( prodname )
if ccomp == None:
callerror("No component with name %s found in CreatePattern" % vals[0])
else:
xa = ivaluate(x_amount)
ya = ivaluate(y_amount)
za = ivaluate(z_amount)
tx = evaluate(x_step)
ty = evaluate(y_step)
tz = evaluate(z_step)
from_range = ivaluate(start_range)
amount = ivaluate(end_range)
start = from_range-1
if xa*ya*za<amount:
amount = xa*ya*za
matrix = vcMatrix.new()
prodid = ccomp.getProperty("ProdID")
if not prodid:
prodid = ccomp.createProperty(VC_STRING,"ProdID")
ccomp.ProdID = prodname
posname = "MW_%s"%(prodid.Value)
target_frame = comp.findFeature(posname)
if target_frame:
target = target_frame.NodePositionMatrix
else:
target = comp.DefaultMatrix #default location
for i in range(start,amount):
newpart = app.cloneComponent(ccomp)
delay(0.0)
cont1.grab(newpart)
delay(0.0)
loc = vcMatrix.new(target)
Pos = loc.P
moveZ=0
level = ya * xa
moveZ = i / level
index2 = i-level*moveZ
ret = divmod(index2, ya)
Pos.X += tx* ret[0]
Pos.Y += ty* ret[1]
Pos.Z += tz* moveZ
loc.P = Pos
newpart.PositionMatrix = loc
stampTAT(newpart, 'CREATED')
if prodid:
ccomp.deleteProperty(prodid)
def negateSurfaceNormals(routine,usemarks):
for s in routine.Statements:
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
m.N=-1.0*m.N
m.A=-1.0*m.A
s.Target = m
def getToolFrames(line):
tool_data_ = []
if not re.findall("LOCAL", line) and re.findall("PERS tooldata", line):
tool_def_ = re.findall(
r"(?P<target>[a-zA-Z0-9_]+)" + ":=" + obracket +
r"(?P<bool>[a-zA-Z]+)" + comma + obracket + obracket +
"(?P<coordx>[0-9\.\-_]+)" + comma + "(?P<coordy>[0-9\.\-_]+)" +
comma + "(?P<coordz>[0-9\.\-_]+)" + cbracket + comma + obracket +
"(?P<orient1>[0-9\.\-_]+)" + comma + "(?P<orient2>[0-9\.\-_]+)" +
comma + "(?P<orient3>[0-9\.\-_]+)" + comma +
"(?P<orient4>[0-9\.\-_]+)" + cbracket, line
) #"\[ *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *\]",
#line)
if tool_def_:
m = vcMatrix.new()
#print "tool %s" %tool_def_[0][2]
tool_name_ = tool_def_[0][0]
m.setQuaternion(float(tool_def_[0][6]), float(tool_def_[0][7]),
float(tool_def_[0][8]), float(tool_def_[0][5]))
coordinates_ = [
float(tool_def_[0][2]),
float(tool_def_[0][3]),
float(tool_def_[0][4]), m.WPR.X, m.WPR.Y, m.WPR.Z
]
#print "coord %s" %coordinates_
tool_data_.append(["Tool", tool_name_, coordinates_])
return tool_data_
def absTranslateTargets(routine,offsetx,offsety,offsetz,usemarks):
if offsetx==0.0 and offsety==0.0 and offsetz==0.0:
return
for s in routine.Statements:
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
m.translateAbs(offsetx,offsety,offsetz)
s.Target = m
def relTranslateTargets(routine,shiftN,shiftO,shiftA,usemarks):
if shiftN==0.0 and shiftO==0.0 and shiftA==0.0:
return
for s in routine.Statements:
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
m.translateRel(shiftN,shiftO,shiftA)
s.Target = m
def setRotatingOrientations(routine,usemarks):
previousroll=0.0
lastnvec=vcVector.new(0.0,0.0,0.0)
indices=xrange(routine.StatementCount)
for idx in indices:
s=routine.getStatement(idx)
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
next=vcMatrix.new(0.0,0.0,0.0)
nvec=vcVector.new(0.0,0.0,0.0)
nextfound=False
nextidx=idx+1
while nextidx<routine.StatementCount:
nextstatement=routine.getStatement(nextidx)
#check that the next statement used for orientation calculation is motion statement
if ((nextstatement.Type == VC_STATEMENT_LINMOTION) or (nextstatement.Type== VC_STATEMENT_PTPMOTION)):
next=vcMatrix.new(nextstatement.Target)
nvec=vcVector.new(next.P-m.P)
#check that the next motion statement is not in same location as current statement
if nvec.length()>0.0:
nvec.normalize()
#check that the next motion statement is not in same line defined by approach vector
if abs(nvec*m.A)<=0.999:
nextfound=True
break
nextidx+=1
if nextfound==False and lastnvec.length()>0.0:
nvec=lastnvec
ovec=vcVector.new(0.0,0.0,0.0)
if nvec.length()>0.0:
# calculate orientation vector from approach and normal, orthonormalize
if abs(nvec*m.A)<=0.999:
ovec=m.A^nvec
ovec.normalize()
nvec=ovec^m.A
nvec.normalize()
lastnvec=nvec
m.N=nvec
m.O=ovec
s.Target = m
def exactPos(part, x, y, z, xx,yy,zz, c, b, a): # Deprecated (exactPos2 replaced this one) mtx = vcMatrix.new() mtx.translateRel(xx,yy,zz) mtx.translateRel(x,y,z) mtx.rotateRelZ(c) mtx.rotateRelY(b) mtx.rotateRelX(a) part.PositionMatrix = mtx
def adjustOrientations(routine,rolldelta,pitchdelta,yawdelta,usemarks):
if rolldelta==0.0 and pitchdelta==0.0 and yawdelta==0.0:
return
for s in routine.Statements:
if ((s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION)) and (usemarks==False or s.getProperty("Mark")!=None):
m = vcMatrix.new(s.Target)
m.rotateRelZ(rolldelta)
m.rotateRelY(pitchdelta)
m.rotateRelX(yawdelta)
s.Target = m
def writeDefineBase(output_file, statement, indentation):
name = statement.Base.Name
m = vcMatrix.new(statement.Position)
motiontarget.BaseName = name
if statement.IsRelative:
m = motiontarget.BaseMatrix * m
p = m.P
q = m.getQuaternion()
output_file.write(
" " * indentation +
"%s:=[FALSE,TRUE,\"\",[[%g,%g,%g],[%g,%g,%g,%g]],[[0,0,0],[1,0,0,0]]];\n"
% (name, p.X, p.Y, p.Z, q.X, q.Y, q.Z, q.W))
def writeDefineTool(output_file, statement, indentation):
name = statement.Tool.Name
m = vcMatrix.new(statement.Position)
motiontarget.ToolName = name
if statement.IsRelative:
m = motiontarget.ToolMatrix * m
p = m.P
q = m.getQuaternion()
load = getToolLoad(name)
output_file.write(" " * indentation +
"%s:=[TRUE,[[%g,%g,%g],[%g,%g,%g,%g]],%s];\n" %
(name, p.X, p.Y, p.Z, q.X, q.Y, q.Z, q.W, load))
def print44MatrixValues(property):
global diags
selObjName = property.Name.split('-')[0]# 获取当前传入参数的组件名
diag = findDiag(selObjName)
#print diag
#print "="*47# 可以打印多行相同的值 解决 清除屏幕 不能答应多行相同的值
if diag:
joints = diag[2]
rootMatrixInW = diag[0].WorldPositionMatrix
for each in joints:
print("="*47)
print( each.Name )
mat = vcMatrix.new( rootMatrixInW )
mat.invert()
eachNode = diag[0].findNode(each.Name)
PrintvcMatrix(mat*eachNode.WorldPositionMatrix) # 打印每一个关节相对机器人root的坐标
def assignTarget(stmt, snum):
global robCnt, pvals
for p in pvals:
(idx, xx, yy, zz, ww, pp, rr, uf, ut, cfg, t1, t2, t3, numEx,
exJvals) = p
if idx == snum:
m = vcMatrix.new()
m.translateAbs(xx, yy, zz)
#print ww,pp,rr
m.setWPR(ww, pp, rr)
stmt.Target = m
if stmt.Type == VC_STATEMENT_PTPMOTION:
stmt.Configuration = cfg
stmt.JointTurns1 = t1
stmt.JointTurns4 = t2
stmt.JointTurns6 = t3
#endif
#stmt.readFromTarget( trg )
baseNum = uf
if baseNum == 0:
pass
#stmt.Base = "*NULL*"
else:
stmt.Base = robCnt.Bases[baseNum - 1].Name
#endif
toolNum = ut
if toolNum == 0:
stmt.Tool = "*NULL*"
else:
stmt.Tool = robCnt.Tools[toolNum - 1].Name
#endif
stmt.Name = 'P%i' % idx
for j in range(numEx):
jname = "ExternalJoint%i" % (j + 1)
for prop in stmt.Properties:
if prop.Name == jname:
prop.Value = exJvals[j]
break
#endif
#endfor
#endfor
return
def MoveImmediateToCurrentLocation():
'''
Type and Description:
---------------------
This user-defined function called by the OnStart() function at the start of every
simulation moves the crane to its current location
Arguments: NONE
----------
Returns: NONE
--------
'''
global crane, robotController
currentToolXPosition = robotController.Tools[
0].Node.WorldPositionMatrix.P.X
currentToolYPosition = robotController.Tools[
0].Node.WorldPositionMatrix.P.Y
currentToolZPosition = robotController.Tools[
0].Node.WorldPositionMatrix.P.Z
newToolXPosition = currentToolXPosition
servicestr = "/CraneService"
method = "/Rest/IDeviceStatusApi/GetDeviceDiagnosticData"
url = IP + servicestr + method
response = restQuery(url, "")
for data in response['Data']:
if data['Key'] == "Xposition":
newToolXPosition = float(data['Value'])
break
newToolXPosition = mapXCoordinate(newToolXPosition)
matrix = mat.new()
vec = vcVector.new(newToolXPosition, currentToolYPosition,
currentToolZPosition)
matrix.P = vec
motionTarget = robotController.createTarget()
motionTarget.MotionType = VC_MOTIONTARGET_MT_JOINT
motionTarget.UseJoints = True
motionTarget.TargetMode = VC_MOTIONTARGET_TM_WORLDTARGET
motionTarget.Target = matrix
robotController.moveImmediate(motionTarget)
def OnStart():
program = getActiveProgram()
if not program:
app.messageBox("No program selected, aborting.", "Warning",
VC_MESSAGE_TYPE_WARNING, VC_MESSAGE_BUTTONS_OK)
return False
#endif
opencmd = app.findCommand("dialogOpen")
uri = ""
ok = True
fileFilter = "FANUC Robot Program files (*.va)|*.va"
opencmd.execute(uri, ok, fileFilter)
if not opencmd.Param_2:
print "No file selected for uploading, aborting command"
return False
#endif
uri = opencmd.Param_1
filename = uri[8:len(uri)]
try:
infile = open(filename, "r")
except:
print "Cannot open file \'%s\' for reading" % filename
return
#endtry
filestring = infile.read()
infile.close()
executor = program.Executor
comp = executor.Component
robCnt = executor.Controller
blocks = {}
lineBuffer = ''
currentSection = ''
for line in filestring.split('\n'):
SYSFRAME = re.match(sysframe + end, line)
if SYSFRAME:
if currentSection: blocks[currentSection] = lineBuffer
lineBuffer = ''
currentSection = SYSFRAME.group(1)
continue
#endif
POSREG = re.match(posreg + end, line)
if POSREG:
if currentSection: blocks[currentSection] = lineBuffer
lineBuffer = ''
currentSection = POSREG.group(1)
continue
#endif
NUMREG = re.match(numreg + end, line)
if NUMREG:
if currentSection: blocks[currentSection] = lineBuffer
lineBuffer = ''
currentSection = NUMREG.group(1)
continue
#endif
lineBuffer += line + '\n'
#endfor
if currentSection: blocks[currentSection] = lineBuffer
numregString = blocks.get('$NUMREG', '')
if numregString:
numregs = re_numreg.finditer(numregString)
for n in numregs:
val = eval(n.group(2))
comment = n.group(3)
if val or comment:
nindex = eval(n.group(1))
prop = comp.createProperty(VC_REAL,
'Registers::R[%i]' % nindex)
prop.Value = val
prop.Group = nindex
#endif
#endfor
#endif
baseString = blocks.get('$MNUFRAME', '')
if baseString:
bases = re_frame.finditer(baseString)
for b in bases:
bindex = eval(b.group(2))
xx = eval(b.group('x'))
yy = eval(b.group('y'))
zz = eval(b.group('z'))
ww = eval(b.group('w'))
pp = eval(b.group('p'))
rr = eval(b.group('r'))
m = vcMatrix.new()
m.translateAbs(xx, yy, zz)
m.setWPR(ww, pp, rr)
if bindex > len(robCnt.Bases):
base = robCnt.addBase()
base.Name = 'Uframe%i' % bindex
#endif
robCnt.Bases[bindex - 1].PositionMatrix = m
#endfor
#endif
toolString = blocks.get('$MNUTOOL', '')
if toolString:
tools = re_frame.finditer(toolString)
for t in tools:
tindex = eval(t.group(2))
xx = eval(t.group('x'))
yy = eval(t.group('y'))
zz = eval(t.group('z'))
ww = eval(t.group('w'))
pp = eval(t.group('p'))
rr = eval(t.group('r'))
m = vcMatrix.new()
m.translateAbs(xx, yy, zz)
m.setWPR(ww, pp, rr)
if tindex > len(robCnt.Tools):
tool = robCnt.addTool()
tool.Name = 'Utool%i' % tindex
#endif
robCnt.Tools[tindex - 1].PositionMatrix = m
#endfor
#endif
posregString = blocks.get('$POSREG', '')
if posregString:
routine = program.findRoutine('PositionRegister')
if routine:
routine.clear()
else:
routine = program.addRoutine('PositionRegister')
posregs = re_posreg.finditer(posregString)
for p in posregs:
pindex = eval(p.group(2))
comment = p.group(3)
xx = eval(p.group('x'))
yy = eval(p.group('y'))
zz = eval(p.group('z'))
ww = eval(p.group('w'))
pp = eval(p.group('p'))
rr = eval(p.group('r'))
cfg = p.group('fut')
if cfg == 'F': cfg = 'F U T'
if cfg == 'N': cfg = 'N U T'
t1 = eval(p.group('t1'))
t2 = eval(p.group('t2'))
t3 = eval(p.group('t3'))
m = vcMatrix.new()
m.translateAbs(xx, yy, zz)
m.setWPR(ww, pp, rr)
stmt = routine.addStatement(VC_STATEMENT_PTPMOTION)
posFrame = stmt.Positions[0]
posFrame.PositionInReference = m
posFrame.Configuration = cfg
try:
posFrame.JointTurns4 = t1
posFrame.JointTurns5 = t2
posFrame.JointTurns6 = t3
except:
pass
if comment:
posFrame.Name = comment
else:
posFrame.Name = routine.Name + '_%i' % pindex
#endif
stmt.createProperty(VC_INTEGER, 'INDEX')
stmt.INDEX = pindex
#endfor
#endif
return True
def createGlobalData(program, global_data_):
executor = program.Executor
comp = executor.Component
robCnt = executor.Controller
prop = []
if not global_data_[0] == []:
for data_ in global_data_[0]:
if len(data_[1]) > 1:
val = data_[1]
if len(data_[1]) == 1:
prop = comp.createProperty(VC_REAL, 'Registers::%s' %
data_[0]) # + '%s' % comment)
else:
# print "v1: %s" % len(v[1])
i = 0
while i < len(data_[1]):
prop.append(
comp.createProperty(
VC_REAL,
'Registers::%s' % data_[0] + "_%s" % i))
prop[i].Value = float(val[i])
i += 1 # + '%s' % comment)
else:
#print "data %s" %data_[1]
val = data_[1][0]
prop = comp.createProperty(VC_REAL, 'Registers::%s' % data_[0])
prop.Value = float(val)
#prop = comp.createProperty(VC_REAL, 'Registers::' +data_[0])
#prop.Value = data_[1]
#prop.Group = nindex
if not global_data_[1] == []:
for data_ in global_data_[1]:
m = vcMatrix.new()
m.translateAbs(data_[2][0], data_[2][1], data_[2][2])
m.setWPR(data_[2][3], data_[2][4], data_[2][5])
if data_[0] == "Base":
if data_[1] > len(robCnt.Bases):
base = robCnt.addBase()
base.Name = data_[1] #'Uframe%i' % data_[1]
# endif
for base in robCnt.Bases:
if base.Name == data_[1]:
base.PositionMatrix = m
#robCnt.Bases[data_[1] - 1].PositionMatrix = m
elif data_[0] == "Tool":
if data_[1] > len(robCnt.Tools):
tool = robCnt.addTool()
tool.Name = data_[1] #'Utool%i' % data_[1]
# endif
for tool in robCnt.Tools:
if tool.Name == data_[1]:
tool.PositionMatrix = m
#robCnt.Tools[data_[1] - 1].PositionMatrix = m
if not global_data_[2] == []:
for data_ in global_data_[2]:
routine = program.findRoutine('POSREG_' + data_[1])
if routine:
routine.clear()
else:
routine = program.addRoutine('POSREG_' + data_[1])
stmt = routine.addStatement(VC_STATEMENT_PTPMOTION)
posFrame = stmt.Positions[0]
if data_[0] == "Cartesian":
m = vcMatrix.new()
m.translateAbs(data_[2][0], data_[2][1], data_[2][2])
m.setWPR(data_[2][3], data_[2][4], data_[2][5])
posFrame.PositionInReference = m
elif data_[0] == "Joint":
mt = robCnt.createTarget()
mt.MotionType = VC_MOTIONTARGET_MT_JOINT
mt.UseJoints = True
jv = mt.JointValues
for i in xrange(len(jv)):
jv[i] = data_[2][i]
mt.JointValues = jv
# convert into cartesian
posFrame.PositionInReference = mt.Target
if not data_[3] == []:
posFrame.Configuration = data_[3]
if data_[4] == 0:
stmt.Base = robCnt.Bases[0]
else:
stmt.Base = robCnt.Bases[data_[4] - 1]
# endif
if data_[5] == 0:
stmt.Tool = robCnt.Tools[0]
else:
stmt.Tool = robCnt.Tools[data_[5] - 1].Name
posFrame.Name = data_[1]
def orientationSmoothing(routine,usemarks):
WINDOWSIZE=1
indices=xrange(routine.StatementCount)
for idx in indices:
#print "loop:",idx
s=routine.getStatement(idx)
if ( (s.Type == VC_STATEMENT_LINMOTION) or (s.Type== VC_STATEMENT_PTPMOTION) ):
# Never smooth reference points. In addition, if the routine contain marked points, then smooths only these points.
if s.getProperty("ReferencePoint")!=None or (usemarks==True and s.getProperty("Mark")==None):
#print "skipping point:",idx
continue
m = vcMatrix.new(s.Target)
previous=vcMatrix.new()
next=vcMatrix.new()
nvec=vcVector.new(m.N)
ovec=vcVector.new(m.O)
previdx=idx-1
windowcount=WINDOWSIZE
while previdx>0 and windowcount>0:
#print "index:",idx,"previdx:",previdx
prevstatement=routine.getStatement(previdx)
if ( (prevstatement.Type == VC_STATEMENT_LINMOTION) or (prevstatement.Type== VC_STATEMENT_PTPMOTION) ):
previous=vcMatrix.new(prevstatement.Target)
nvec=nvec+previous.N
ovec=ovec+previous.O
windowcount-=1
previdx-=1
nextidx=idx+1
windowcount=WINDOWSIZE
while nextidx<routine.StatementCount and windowcount>0:
#print "index:",idx,"nextidx:",nextidx
nextstatement=routine.getStatement(nextidx)
if ( (nextstatement.Type == VC_STATEMENT_LINMOTION) or (nextstatement.Type== VC_STATEMENT_PTPMOTION) ):
next=vcMatrix.new(nextstatement.Target)
nvec=nvec+next.N
ovec=ovec+next.O
windowcount-=1
nextidx+=1
#print idx,"nvec",nvec.X,nvec.Y,nvec.Z
#print idx,"ovec",ovec.X,ovec.Y,ovec.Z
nvec.normalize()
ovec.normalize()
if abs(ovec*m.A)>0.999:
ovec=m.A^nvec
ovec.normalize()
nvec=ovec^m.A
nvec.normalize()
else:
nvec=ovec^m.A
nvec.normalize()
ovec=m.A^nvec
ovec.normalize()
m.N=nvec
m.O=ovec
s.Target = m
def importPath(app, filename, subroutine):
global program, routine, controller, basename, toolname, basenames, toolnames, robotconfig, robotconfigs
program, routine, controller, basename, toolname, basenames, toolnames, robotconfig, robotconfigs = getSelectedRobotData(
)
routine = subroutine
program = getProgram()
controller = program.Executor.Controller
infile = open(filename, "r")
try:
infile = open(filename, "r")
except:
app.messageBox("Cannot open file \'%s\' for reading" % filename,
"Warning", VC_MESSAGE_TYPE_WARNING,
VC_MESSAGE_BUTTONS_OK)
return False
defaultparams = app.findLayoutItem("DefaultParameters")
feedrate = 100 #defaultparams.DefaultLINProcessSpeed
#robtarget regular expression
robtargetregex = "\[ *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *\]"
# floating point regular expression
floatregex = "[-+]?[0-9]*\.?[0-9]+(?:[eE][-+]?[0-9]+)?"
# workobject regular expression
wobjregex = r"\\WObj:=(?P<workobject>[a-zA-Z0-9]+)"
# tool definition regular expression
toolregex = r", *(?P<tooln>[a-zA-Z0-9]+)"
curlc = 0
lc = get_file_linecount(infile)
prevtime = time.clock()
firstwarning = True
while True:
#Update line count
curlc += 1
#Read nci command
myline = infile.readline()
if not myline: break
rtmatch = re.findall(robtargetregex, myline)
for mymatch in rtmatch:
match = re.findall(floatregex, mymatch)
# robtarget has 17 floating point values
if len(match) == 17:
m = vcMatrix.new()
m.P = vcVector.new(float(match[0]), float(match[1]),
float(match[2]))
m.setQuaternion(float(match[4]), float(match[5]),
float(match[6]), float(match[3]))
t = controller.createTarget(m)
if "MoveJ" in myline.lower():
s = routine.addStatement(VC_STATEMENT_PTPMOTION)
else:
s = routine.addStatement(VC_STATEMENT_LINMOTION)
#cfx config
t.RobotConfig = int(match[10])
#joint turns for axis 1,4 and 6
if s.Type == VC_STATEMENT_PTPMOTION:
jointturnprops = [
"JointTurns1", "JointTurns4", "JointTurns6"
]
configindex = 7
for jointturnpropname in jointturnprops:
jointturnprop = s.getProperty(jointturnpropname)
if jointturnprop != None:
if float(match[configindex]) > 1:
jointturnprop.Value = 1
elif float(match[configindex]) < -2:
jointturnprop.Value = -1
configindex += 1
s.readFromTarget(t)
#set base based on workobject definition
wobjdef = re.search(wobjregex, myline)
if wobjdef:
wobjname = wobjdef.group('workobject')
if wobjname:
#set base for statement only if the base is found from controller
for b in controller.Bases:
if b.Name == wobjname:
s.Base = wobjname
if s.Base != wobjname and firstwarning:
firstwarning = False
app.messageBox(
"Undefined workobject \'%s\' in robot program"
% wobjname, "Warning", VC_MESSAGE_TYPE_WARNING,
VC_MESSAGE_BUTTONS_OK)
#tool definition
tooldef = None
for tooldef in re.finditer(toolregex, myline):
pass
if tooldef:
rtoolname = tooldef.group('tooln')
#set base for statement only if the base is found from controller
for b in controller.Tools:
if b.Name == rtoolname:
s.Tool = rtoolname
if s.Tool != rtoolname and firstwarning:
firstwarning = False
app.messageBox(
"Undefined tool \'%s\' in robot program" %
rtoolname, "Warning", VC_MESSAGE_TYPE_WARNING,
VC_MESSAGE_BUTTONS_OK)
# external axes
for j in range(6):
if match[j + 11] != "9E+09" and match[j + 11] != "9e+09":
extjoint = s.getProperty("ExternalJoint" + str(j + 1))
if extjoint != None:
extjoint.Value = float(match[j + 11])
else:
firstwarning = False
app.messageBox(
"Undefined external axis \'%s\' in robot program"
% (j + 1), "Warning", VC_MESSAGE_TYPE_WARNING,
VC_MESSAGE_BUTTONS_OK)
if s.Type == VC_STATEMENT_LINMOTION:
s.Acceleration = 0
s.Deceleration = 0
s.AngularAcceleration = 0
s.AngularDeceleration = 0
s.MaxSpeed = feedrate
# Update progress
curtime = time.clock()
if curtime - prevtime > 2.0:
prevtime = curtime
myprogress = float(curlc) / float(lc + 1)
app.ProgressStatus = "Importing targets: %d%% done " % int(
100.0 * myprogress)
app.ProgressValue = myprogress
infile.close()
return True
def first_state():
print "Importing machine data..."
componentname = getProperty("ComponentName").Value
filename = getProperty("FileName").Value
ErrorProperty = getProperty("ErrorString")
if DEBUG_MESSAGES:
print "componentname = %s" % componentname
print "filename = %s" % filename
rcomp = getApplication().findComponent(componentname)
if not rcomp:
ErrorProperty.Value = 'Component \'%\' not found, aborting command' % ComponentName
print ErrorProperty.Value
return
track = getTrack(rcomp)
if DEBUG_MESSAGES:
if track:
print "track found = %s" % track.Name
else:
print "no track found"
global external_comps
external_comps = []
# Get robot controller
robot = GetRobotController(rcomp)
# Get RSL executor
executor = GetRslExecutor(rcomp)
if robot == None:
ErrorProperty.Value = 'Robot not found, aborting command'
print ErrorProperty.Value
return
if DEBUG_MESSAGES:
print "controller found = %s" % robot.Name
if executor:
print "executor found = %s" % executor.Name
else:
print "no executor found"
if filename == "":
ErrorProperty.Value = 'Machine data file not found, aborting command'
print ErrorProperty.Value
return
try:
infile = open(filename, "r")
except:
ErrorProperty.Value = 'Cannot open machine data file %s, aborting command' % filename
print ErrorProperty.Value
return
#endtry
filestring = infile.read()
infile.close()
# Get external joints and connected components
for j in robot.Joints:
if j.ExternalController != None:
exists = False
for i in external_comps:
if i.Name == j.Controller.Component.Name:
exists = True
break
if exists == False:
external_comps.append(j.Controller.Component)
#endif
#endfor
for comp in external_comps:
prop = comp.getProperty('EX_KIN')
if prop == None:
ErrorProperty.Value = "EX_KIN property not found from component, aborting command"
print ErrorProperty.Value
return
#endif
ex_kin = prop.Value
# Update name property: $ET3_NAME[]="DKP 400-1 #2"
re_name = re.compile('\$' + ex_kin + r'_NAME\[\]=(?P<name>.*)',
re.M + re.IGNORECASE)
et_name = re_name.finditer(filestring)
for i in et_name:
comp_name = i.group('name')
comp_name = comp_name.replace('"', '')
#UpdateBaseFrameStatements(executor, comp.Name, comp_name)
comp.Name = comp_name
if DEBUG_MESSAGES:
print "updated component name to %s" % comp_name
break
#endfor
# Update kinematics definition: DECL EX_KIN $EX_KIN={ET1 #EASYS,ET2 #NONE,ET3 #NONE,ET4 #NONE,ET5 #NONE,ET6 #NONE}
startIndex = filestring.find('DECL EX_KIN $EX_KIN={')
if startIndex < 0:
ErrorProperty.Value = "Error parsing machine data (kinematic definition), aborting command"
print ErrorProperty.Value
return
#endif
endIndex = filestring.find('}', startIndex)
old_kin_definition = filestring[startIndex:endIndex + 1]
startIndex = old_kin_definition.find(ex_kin)
et_definition = old_kin_definition[startIndex:old_kin_definition.
find(',', startIndex)]
et_definition = et_definition.replace(ex_kin, '').lstrip().rstrip()
et = comp.getProperty('ET')
if et != None:
et.Value = et_definition
if DEBUG_MESSAGES:
print "updated ET value to %s" % et_definition
#endif
# Update exis definition: DECL ET_AX $ET1_AX={TR_A1 #E1,TR_A2 #E2,TR_A3 #NONE}
ax_definition = r'\$' + ex_kin + '_AX={.*'
re_ax_definition = re.compile(ax_definition, re.M + re.IGNORECASE)
ax_defs = re_ax_definition.finditer(filestring)
if ax_defs == None:
ErrorProperty.Value = "Error parsing machine data (axis definitions), aborting command"
print ErrorProperty.Value
return
#endif
for i in ax_defs:
from_str = i.group()
to_str = from_str
tr_a1 = comp.getProperty('TR_A1')
if tr_a1 != None:
tr_a1_str = from_str[from_str.find('TR_A1'):from_str.
find(',', from_str.find('TR_A1'))]
tr_a1_str = tr_a1_str.replace('TR_A1', '')
tr_a1_str = tr_a1_str.lstrip().rstrip()
tr_a1.Value = tr_a1_str
if DEBUG_MESSAGES:
print "updated TR_A1 value to %s" % tr_a1_str
#endif
tr_a2 = comp.getProperty('TR_A2')
if tr_a2 != None:
tr_a2_str = from_str[from_str.find('TR_A2'):from_str.
find(',', from_str.find('TR_A2'))]
tr_a2_str = tr_a2_str.replace('TR_A2', '')
tr_a2_str = tr_a2_str.lstrip().rstrip()
tr_a2.Value = tr_a2_str
if DEBUG_MESSAGES:
print "updated TR_A2 value to %s" % tr_a2_str
#endif
#endfor
num = r' *-?\d*\.?\d+(?:[eE][-+]?\d+)?'
joint_offset = r'\$' + ex_kin + r'_(?P<name>.*)={X (?P<X>' + num + '),Y (?P<Y>' + num + '),Z (?P<Z>' + num + '),A (?P<A>' + num + '),B (?P<B>' + num + '),C (?P<C>' + num + ')}.*'
re_joint_offset = re.compile(joint_offset, re.M + re.IGNORECASE)
joint_offsets = re_joint_offset.finditer(filestring)
if joint_offsets == None:
ErrorProperty.Value = "Error parsing machine data (joint offsets), aborting command"
print ErrorProperty.Value
return
#endif
for jo in joint_offsets:
fp = comp.getProperty(jo.group('name'))
if fp != None:
m = vcMatrix.new()
m.P = vcVector.new(eval(jo.group('X')), eval(jo.group('Y')),
eval(jo.group('Z')))
m.WPR = vcVector.new(eval(jo.group('C')), eval(jo.group('B')),
eval(jo.group('A')))
fp.Value = m
if DEBUG_MESSAGES:
print "updated value of %s" % fp.Name
#endif
#endfor
#$RAT_MOT_AX[7]={N -346,D 10}
gear_ratio = r'\$RAT_MOT_AX\[(?P<index>' + num + ')\]={N (?P<N>' + num + '),D (?P<D>' + num + ')}.*'
re_gear_ratio = re.compile(gear_ratio, re.M + re.IGNORECASE)
gear_ratios = re_gear_ratio.finditer(filestring)
if gear_ratios == None:
ErrorProperty.Value = "Error parsing machine data (gear ratios), aborting command"
print ErrorProperty.Value
return
#endif
for gr in gear_ratios:
fp = comp.getProperty('RAT_MOT_AX_' + gr.group('index'))
if fp != None:
fp.Value = float(eval(gr.group('N'))) / float(
eval(gr.group('D')))
if DEBUG_MESSAGES:
print "updated value of " % fp.Name
# this property is present only in case of a track (?), therefore, also mark the robot mounting to be be 'Custom'
rm = comp.getProperty('RobotMounting')
if rm:
rm.Value = 'Custom'
#endif
#endfor
#endfor
print "Done!"
def WriteStatement(mod, statement):
global motiontarget, statementCount, pointCount
#Add missing property for positioning path
#if (statement.Type == VC_STATEMENT_LINMOTION or statement.Type == VC_STATEMENT_PTPMOTION) and statement.getProperty("PosPath")==None:
# addProperty(statement, VC_STRING, "PosPath",True,False,False,defaultparams.PosPath,None,"")
statementCount += 1
mod.write("%4i:" % statementCount)
if statement.Type == VC_STATEMENT_CALL:
mod.write(" CALL %s ;\n" %
statement.getProperty("Routine").Value.Name)
elif statement.Type == VC_STATEMENT_COMMENT:
c = statement.Comment
mod.write(" !%s;\n" % (c))
elif statement.Type == VC_STATEMENT_DELAY:
d = statement.Delay
mod.write(" WAIT %6.2f(sec) ;\n" % (d))
elif statement.Type == VC_STATEMENT_HALT:
mod.write(" PAUSE ;\n")
elif statement.Type == VC_STATEMENT_LINMOTION:
statement.writeToTarget(motiontarget)
pointCount += 1
mod.write("L P[%i: %s] %gmm/sec %s ;\n" %
(pointCount, statement.Positions[0].Name, statement.MaxSpeed,
defaults['PosPath']))
elif statement.Type == VC_STATEMENT_PTPMOTION:
statement.writeToTarget(motiontarget)
pointCount += 1
mod.write("J P[%i: %s] %g%% %s ;\n" %
(pointCount, statement.Positions[0].Name,
statement.JointSpeed, defaults['PosPath']))
elif statement.Type == VC_STATEMENT_SETBIN:
mod.write(" DO[%i: %s]= " % (statement.OutputPort, statement.Name))
if statement.OutputValue:
mod.write("ON ;\n")
else:
mod.write("OFF ;\n")
elif statement.Type == VC_STATEMENT_WAITBIN:
mod.write(" WAIT DI[%i: %s]= " %
(statement.InputPort, statement.Name))
if statement.InputValue:
mod.write("ON ;\n")
else:
mod.write("OFF ;\n")
elif statement.Type == VC_STATEMENT_DEFINE_BASE:
name = statement.Base.Name
m = vcMatrix.new(statement.Position)
motiontarget.BaseName = name
if statement.IsRelative == 1:
m = motiontarget.BaseMatrix * m
p = m.P
a = m.getWPR()
mod.write(" !DEFINE BASE ;\n")
elif statement.Type == VC_STATEMENT_DEFINE_TOOL:
name = statement.Tool.Name
m = vcMatrix.new(statement.Position)
motiontarget.ToolName = name
if statement.IsRelative == 1:
m = motiontarget.ToolMatrix * m
p = m.P
a = m.getWPR()
mod.write(" !DEFINE TOOL ;\n")
elif statement.Type == VC_STATEMENT_PROG_SYNC:
mod.write(" !PROGSYNC ;\n")
else:
mod.write(" ;\n")
def getGlobPos(line):
move_data_ = None
if not re.findall("LOCAL", line) and re.findall("PERS robtarget", line):
rob_target_def = re.findall(
r"(?P<target>[a-zA-Z0-9_]+)" + ":=" +
"\[ *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *\]",
line)
targetname = rob_target_def[0]
positionmatch = re.findall(
"\[ *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *, *\[[0-9+-eE,. ]+\] *\]",
line)
# print "target: %s" % targetname
# print "positionmatch: %s" % positionmatch
# for mymatch in positionmatch:
match = re.findall("[-+]?[0-9]*\.?[0-9]+(?:[eE][-+]?[0-9]+)?",
positionmatch[0])
# print "match: %s" % match
# robtarget has 17 floating point values
if len(match) == 17:
m = vcMatrix.new()
# m.P = vcVector.new(float(match[0]),float(match[1]),float(match[2]))
m.setQuaternion(float(match[4]), float(match[5]), float(match[6]),
float(match[3]))
coordinates_ = [
float(match[0]),
float(match[1]),
float(match[2]), m.WPR.X, m.WPR.Y, m.WPR.Z
]
config_data_ = [match[7], match[8], match[9], match[10]]
#tool_ = getTool(line)
#base_ = getBase(line)
#speed_ = getSpeed(line)
move_data_ = ["Cartesian", targetname, coordinates_, [], 0, 17]
elif not re.findall("LOCAL", line) and re.findall("PERS jointtarget",
line):
rob_target_def = re.findall(
r"(?P<target>[a-zA-Z0-9_]+)" + ":=" + obracket + obracket +
"(?P<joint1>[0-9\.\-_]+)" + comma + "(?P<joint2>[0-9\.\-_]+)" +
comma + "(?P<joint3>[0-9\.\-_]+)" + comma +
"(?P<joint4>[0-9\.\-_]+)" + comma + "(?P<joint5>[0-9\.\-_]+)" +
comma + "(?P<joint6>[0-9\.\-_]+)" + cbracket, line)
#line)
#print "robtarget %s" %rob_target_def
if rob_target_def:
targetname = rob_target_def[0]
#print "target: %s" % targetname[0]
coordinates_ = [
float(targetname[1]),
float(targetname[2]),
float(targetname[3]),
float(targetname[4]),
float(targetname[5]),
float(targetname[6])
]
config_data_ = []
move_data_ = [
"Joint", targetname[0], coordinates_, config_data_, 0, 17
]
return move_data_
def OnRun():
global parts, pallets, comp, customPos, pendingPallets, convTransSignal, trigConts
try:
robo = getRobot('RobotInterface')
#print robo.__doc__
except:
warning('No robot attached on the controller','Place robot on component:'+comp.Name)
suspendRun()
comp = getComponent()
setRoboSpeeds(robo, comp)
inConvIface = comp.findBehaviour('InConvInterface')
outConvIface = comp.findBehaviour('OutConvInterface')
partInSignal = comp.findBehaviour('ArriveComponentSignal')
palletInSignal = comp.findBehaviour('ArrivePalletSignal')
convTransSignal = comp.findBehaviour('ConvTransSignal')
inConvs = []
outConvs = []
for sec in inConvIface.ConnectedToSections:
inConvs.append(sec.Interface.Component)
for sec in outConvIface.ConnectedToSections:
outConvs.append(sec.Interface.Component)
if not inConvs:
warning('No input conveyor connected','Connect in conveyor on param tab.')
suspendRun()
if not outConvs:
warning('No output conveyor connected','Connect out conveyor on param tab.')
suspendRun()
trigConts = []
#for oConv in outConvs:
# for cont in oConv.findBehavioursByType(VC_CONTAINER):
# trigConts.append(cont)
# trigConts[-1].OnTransition = contTrans
parts = []
prevparts = []
pallets = []
pendingPallets = []
again = False
robo.ActiveTool = comp.getProperty('Advanced::ToolIndex').Value
'''
robo.Speed = 3000
robo.AngularSpeed = 3000
robo.Acc = 6000
robo.AngularAcc = 6000
robo.RecordRSL = True
'''
runner = 0
if patternType.Value == 'Custom':
patternNote = comp.findBehaviour('Pattern::CustomPattern')
customPos = getCustomPositions(patternNote.Note)
#
#
#
# WORK LOOP STARTS
#
while True:
if not again:
triggerCondition(lambda: True)
again = True # Force logic to loop again, coz there's a change in environment
else:
again = False
partid = ''
# ###############################################
# GET PENDING PALLET FROM PALLET INLET
# ###############################################
if placeParts.Value == 'OnPallets' and pendingPallets:
for outConv in outConvs:
picked = False
if not outConv.ChildComponents:
ifaces = outConv.findBehavioursByType(VC_ONETOONEINTERFACE)
inputIface = None
for iface in ifaces:
for sec in iface.Sections:
for field in sec.Fields:
if field.Type == VC_FLOWFIELD:
if field.Container.Connectors[field.Port].Type == VC_CONNECTOR_INPUT or field.Container.Connectors[field.Port].Type == VC_CONNECTOR_INPUT_OUTPUT:
inputIface = iface
if not inputIface.IsConnected:
#condition(lambda: pendingPallets)
if comp.getProperty('PalletID Filtering').Value:
inPallet = pendingPallets[0]
if readPalletID.Value == 'From Pallet':
inPalletProdIDs = inPallet.ProdID
else:
inPalletProdIDs = inPallet.Parent.ProdID_Filter
prodIDS = outConv.ProdID_Filter
prodIDS = prodIDS.replace(';',',')
inPalletProdIDs = inPalletProdIDs.replace(';',',')
inPalletProdIDs = inPalletProdIDs.split(',')
ids = prodIDS.split(',')
for inPalletProdID in inPalletProdIDs:
if inPalletProdID in ids:
pendingPallets.pop(0)
robo.pick(inPallet)
robo.place(outConv)
picked = True
if not inPallet in pallets:
pallets.append(inPallet)
else:
inPallet = pendingPallets.pop(0)
robo.pick(inPallet)
robo.place(outConv)
picked = True
if not inPallet in pallets:
pallets.append(inPallet)
if picked == True:
break
targetPart = None
targetPallet = None
targetConv = None
if not parts:
idlePos = comp.getProperty('Advanced::RobotIdlePos Z').Value
robo.jointMoveToComponent(inConvs[0], Tz = idlePos, OnFace = 'top')
# ###################################################################
# SEARCH TARGETPART AND TARGETPALLET/-CONVEYOR
# ###################################################################
############################################
## PARTS GOES ON PALLETS AND PRODID FILTERING IS ENABLED
############################################
if placeParts.Value == 'OnPallets' and prodIDFiltering.Value:
#condition(lambda: parts != prevparts and pallets)
prevparts = parts[:]
delay(0.00001)
breakk = False
for ipallet in pallets:
for ipart in parts:
if readPalletID.Value == 'From Pallet':
ipallet.ProdID = ipallet.ProdID.replace(';',',')
palletIDS = ipallet.ProdID.split(',')
else:
ipallet.Parent.ProdID_Filter = ipallet.Parent.ProdID_Filter.replace(';',',')
palletIDS = ipallet.Parent.ProdID_Filter.split(',')
##
## SUBCASE PARTS ARRIVES ON PALLETS
##
if partsOnPallet.Value:
kids = ipart.ChildComponents
if inversedPickOrder.Value:
kids.reverse()
for jpart in kids:
if readPartID.Value == 'FromPart':
partid = jpart.ProdID
elif readPartID.Value == 'FromConveyor':
partid = ipart.Parent.ProdID_Filter
elif readPartID.Value == 'FromPallet':
partid = ipart.ProdID
if partid in palletIDS:
targetPart = jpart
targetPallet = ipallet
fromPallet = ipart
prevparts = ['CHANGED']
breakk = True
break
if breakk:
break
##
## SUBCASE PARTS ARRIVE AS INDIVIDUAL PARTS
##
else:
if readPartID.Value == 'FromPart':
partids = [ipart.ProdID]
elif readPartID.Value == 'FromConveyor':
ipart.Parent.ProdID_Filter = ipart.Parent.ProdID_Filter.replace(';',',')
partids = ipart.Parent.ProdID_Filter.split(',')
elif readPartID.Value == 'FromPallet':
partids = [ipart.ProdID]
for partid in partids:
if partid in palletIDS:
prevparts = ['CHANGED']
targetPart = ipart
targetPallet = ipallet
parts.remove(targetPart)
breakk = True
break
if breakk:
break
if breakk:
break
###################################################
## PARTS GOES DIRECTLY ON CONVEYORS AND PRODID FILTERING IS ENABLED
###################################################
elif placeParts.Value == 'OnConveyor' and prodIDFiltering.Value:
#condition(lambda: parts != prevparts)
prevparts = parts[:]
breakk = False
for ipart in parts:
for iconv in outConvs:
iconv.ProdID_Filter = iconv.ProdID_Filter.replace(';',',')
convIDS = iconv.ProdID_Filter.split(',')
##
## SUBCASE PARTS ARRIVES ON PALLETS
##
if partsOnPallet.Value:
kids = ipart.ChildComponents
if inversedPickOrder.Value:
kids.reverse()
for jpart in kids:
if readPartID.Value == 'FromPart':
partid = jpart.ProdID
elif readPartID.Value == 'FromConveyor':
partid = ipart.Parent.ProdID_Filter
elif readPartID.Value == 'FromPallet':
partid = ipart.ProdID
if partid in convIDS:
targetPart = jpart
fromPallet = ipart
targetConv = iconv
prevparts = ['CHANGED']
breakk = True
break
if breakk:
break
##
## SUBCASE PARTS ARRIVE AS INDIVIDUAL PARTS
##
else:
if readPartID.Value == 'FromPart':
partid = ipart.ProdID
elif readPartID.Value == 'FromConveyor':
partid = ipart.Parent.ProdID_Filter
if partid in convIDS:
targetPart = ipart
targetConv = iconv
parts.remove(targetPart)
prevparts = ['CHANGED']
breakk = True
break
if breakk:
break
#########################
## PRODID FILTERING IS DISABLED
#########################
elif not prodIDFiltering.Value:
#condition(lambda: parts != prevparts)
prevparts = parts[:]
##
## SUBCASE PARTS ARRIVES ON PALLETS
##
if partsOnPallet.Value:
kids = None
if parts:
ipart = parts[0]
fromPallet = ipart
kids = ipart.ChildComponents
if kids:
prevparts = ['CHANGED']
if inversedPickOrder.Value:
targetPart = kids[-1]
else:
targetPart = kids[0]
##
## SUBCASE PARTS ARRIVE AS INDIVIDUAL PARTS
##
else:
if placeParts.Value == 'OnPallets':
if parts and pallets:
targetPart = parts.pop(0)
targetPallet = pallets[0]
else:
if parts:
targetPart = parts.pop(0)
## Choose simply the oldest pallet as a target pallet (FIFO)
if placeParts.Value == 'OnPallets' and pallets:
targetPallet = pallets[0]
# ###################################################################
# ###################################################################
# TARGETPART AND TARGETPALLET/-CONVEYOR ARE ALREADY SEARCHED
# LET'S TEST IF SOME OF THE FOLLOWING CASES ARE TRUE
# ###################################################################
# ###################################################################
caseA = placeParts.Value == 'OnPallets' and prodIDFiltering.Value and targetPart and targetPallet
caseB = placeParts.Value == 'OnPallets' and not prodIDFiltering.Value and targetPart and targetPallet
caseC = placeParts.Value == 'OnConveyor' and prodIDFiltering.Value and targetPart and targetConv
caseD = placeParts.Value == 'OnConveyor' and not prodIDFiltering.Value and targetPart
if caseA or caseB or caseC or caseD:
##
## Pick part
##
# Part sizes
partSize = targetPart.Geometry.BoundDiagonal*2.0
partSizeX = partSize.X
partSizeY = partSize.Y
partSizeZ = partSize.Z
aX,aY,aZ = getApproach('Advanced::ApproachOnPart')
robo.jointMoveToComponent(targetPart, Tx = aX, Ty = aY, Tz = aZ+partSizeZ)
robo.pickMovingPart(targetPart,Approach = aZ)
again = True # Force logic to loop again, coz there's a change in environment
# if part was on pallet try to start its pallet if it's now empty
if partsOnPallet.Value:
if fromPallet and len(fromPallet.ChildComponents) - comp.getProperty('Pattern::PalletChildsOnArrive').Value <= 0:
fromPallet.startMovement()
if fromPallet in parts:
parts.remove(fromPallet)
##
## Place part
##
# Part placing on pallet
if placeParts.Value == 'OnPallets':
# move on top of conveyor if idle
if not prodIDFiltering.Value:
if not pallets: # Go on top of out conv to wait next pallet
idlePos = comp.getProperty('Advanced::RobotIdlePos Z').Value
robo.jointMoveToComponent(outConvs[0], Tz = idlePos, OnFace = 'top')
#condition(lambda: pallets) # wait pallet
childCount = len(targetPallet.Children) - comp.getProperty('Pattern::PalletChildsOnArrive').Value # Count pallet childs
if patternType.Value == 'XYZ':
x, y, z = getPatternSizes(targetPallet)
cloneIndex, patternIndex = divmod( childCount, getFullPatternSize(x,y,z,partid) )
else:
cloneIndex, patternIndex = divmod( childCount, getFullPatternSize(id = partid) )
# Pallet sizes
palletSize = targetPallet.Geometry.BoundDiagonal*2.0
palletSizeX = palletSize.X
palletSizeY = palletSize.Y
palletSizeZ = palletSize.Z
if patternType.Value == 'XYZ':
xStep, yStep, zStep = getPatternSteps(targetPallet,targetPart)
k, ij = divmod(patternIndex,y*x)
i, j = divmod(ij,y)
placeX = -( (x-1) * xStep ) /2 + i*xStep #+ xx
placeY = -( (y-1) * yStep ) /2 + j*yStep #+ yy
placeZ = k * (2*zStep) + palletSizeZ #+ zz
a,b,c = 0.0, 0.0, 0.0
else: # PatternType == 'Custom'
x,y,z = 0,0,0
if patternPerProdID.Value:
if not partid:
warning('Use "PatternPerProdID" with "ProdID_Filtering" enabled', 'Enable "ProdID_Filtering" on General-tab')
partid = targetPart.ProdID
placeX = customPos[partid][patternIndex][0]
placeY = customPos[partid][patternIndex][1]
placeZ = customPos[partid][patternIndex][2]
a = customPos[partid][patternIndex][3]
b = customPos[partid][patternIndex][4]
c = customPos[partid][patternIndex][5]
else:
placeX = customPos['GeneralPattern'][patternIndex][0]
placeY = customPos['GeneralPattern'][patternIndex][1]
placeZ = customPos['GeneralPattern'][patternIndex][2]
a = customPos['GeneralPattern'][patternIndex][3]
b = customPos['GeneralPattern'][patternIndex][4]
c = customPos['GeneralPattern'][patternIndex][5]
clonePos = vcMatrix.new()
clonePos.translateRel(0,0,cloneIndex * cloneStep.Value)
if cloneRotationType.Value == 'Increasing':
cloneRot = cloneIndex * cloneRotation.Value
else:
rr,ee = divmod(cloneIndex,2)
if ee:
cloneRot = cloneRotation.Value
else:
cloneRot = 0.0
clonePos.rotateRelZ(cloneRot)
offsetmtx = comp.getProperty('PatternPosition::Position').Value
offsetmtx = offsetmtx * clonePos
v = offsetmtx.getWPR()
xx =offsetmtx.P.X
yy = offsetmtx.P.Y
zz = offsetmtx.P.Z
mtx = vcMatrix.new()
aX,aY,aZ = getApproach('Advanced::ApproachOnPallet')
targetPallet.update()
w = targetPallet.WorldPositionMatrix
targetMtx = w*offsetmtx
targetMtx.translateRel(placeX,placeY,placeZ+partSizeZ)
targetMtx.rotateRelZ(c)
targetMtx.rotateRelY(b)
targetMtx.rotateRelX(a)
#robo.jointMoveToMtx(targetMtx, Tx = aX, Ty = aY, Tz = aZ, Rx = 180 + a, Ry = b, Rz = c)
# Move close
robo.jointMoveToMtx(targetMtx, Tx = aX, Ty = aY, Tz = aZ, Rx = 180.0)
targetMtx = offsetmtx
#targetMtx.translateRel(placeX,placeY,placeZ+partSizeZ)
targetMtx.translateRel(placeX,placeY,placeZ)
targetMtx.rotateRelZ(c)
targetMtx.rotateRelY(b)
targetMtx.rotateRelX(a) # Now the matrix is in the exact position of the part origin in target
exactMtx = vcMatrix.new(targetMtx)
targetMtx.translateRel(0,0,partSizeZ) # Let's retract TCP with the part height amount
# Move on part
robo.linearMoveToMtx_ExternalBase(targetPallet,offsetmtx, Rx = 180.0)
robo.releaseComponent(targetPallet)
#exactPos(targetPart, placeX, placeY, placeZ, xx,yy,zz,v.Z+c, v.Y+b, v.X+a)
exactPos2(targetPart,exactMtx)
robo.linearMoveRel(Tx= -aX,Ty= -aY, Tz= -aZ)
if comp.StopPallets:
if patternPerProdID.Value:
notFull = patternNotFull(targetPallet,x,y,z, partid)
else:
notFull = patternNotFull(targetPallet,x,y,z, id = None)
if not notFull:
targetPallet.startMovement()
if targetPallet in pallets:
pallets.remove(targetPallet)
# Part Placing On Conveyor
elif placeParts.Value == 'OnConveyor':
if not prodIDFiltering.Value:
offsetmtx = comp.getProperty('PatternPosition::Position').Value
v = offsetmtx.getWPR()
xx = offsetmtx.P.X
yy = offsetmtx.P.Y
zz = offsetmtx.P.Z
aX,aY,aZ = getApproach('Advanced::ApproachOnPallet')
robo.place(outConvs[0], Tx = xx, Ty = yy, Tz = zz, Rx = v.X, Ry = v.Y, Rz = v.Z, Approach = aZ)
else:
offsetmtx = comp.getProperty('PatternPosition::Position').Value
v = offsetmtx.getWPR()
xx = offsetmtx.P.X
yy = offsetmtx.P.Y
zz = offsetmtx.P.Z
aX,aY,aZ = getApproach('Advanced::ApproachOnPallet')
robo.place(targetConv, Tx = xx, Ty = yy, Tz = zz, Rx = v.X, Ry = v.Y, Rz = v.Z, Approach = aZ)
