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)