import TK vismach to QTVCP

#! /usr/bin/python2 #    Copyright 2007 John Kasunich and Jeff Epler # #    This program is free software; you can redistribute it and/or modify #    it under the terms of the GNU General Public License as published by #    the Free Software Foundation; either version 2 of the License, or #    (at your option) any later version. # #    This program is distributed in the hope that it will be useful, #    but WITHOUT ANY WARRANTY; without even the implied warranty of #    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the #    GNU General Public License for more details. # #    You should have received a copy of the GNU General Public License #    along with this program; if not, write to the Free Software #    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. from .qt_vismach import * import hal import math import sys c = None METRIC = 1 IMPERIAL = 25.4 MODEL_SCALING = IMPERIAL # parameters that define the geometry see scarakins.c for definitions these # numbers match the defaults there, and will need to be changed or specified on # the commandline if you are not using the defaults. #setp scarakins.D1 400.0 #setp scarakins.D2 350.0 #setp scarakins.D3 -75.0 #setp scarakins.D4 350.0 ##setp scarakins.D5 200.0 #setp scarakins.D6 0.0 d1 =  400.0 d2 =  350.0 d3 =   175.0 d4 =  350.0 d5 =   200.0 d6 =   0.0 j3min =  40.0 j3max = 270.0 class HalToolCylinder(CylinderZ):     def __init__(self, comp, *args):         # get machine access so it can         # change itself as it runs         # specifically tool cylinder in this case.         CylinderZ.__init__(self, *args)         self.comp = c     def coords(self):         # update data -  not needed if using 2.8 and self.comp["tooldiameter"]         # 2.7 does not have direct pin for diameter so this is workaround. commented out code is direct way to do it.         # s.poll() # 2.8 don't need this, comment out if using 2.8.         # get diameter and divide by 2 to get radius.         # rad = ( s.tool_table[s.tool_in_spindle].diameter ) # 2.7 workaround         try:             dia = (hal.get_value('halui.tool.diameter') * MODEL_SCALING)         except:             dia = 0         rad = dia / 2  # change to rad         # this instantly updates tool model but tooltip doesn't move till -         # tooltip, the drawing point will NOT move till g43h(tool number) is called, however.         # Tool will "crash" if h and tool length does not match.         try:             leng = hal.get_value('motion.tooloffset.z') * MODEL_SCALING         except:             leng = 0         # Update tool length when g43h(toolnumber) is called, otherwise stays at 0 or previous size.         # commented out as I prefer machine to show actual tool size right away.         # leng = self.comp["toollength"]         return (-leng, rad, 0, rad) # calculate a bunch of other dimensions that are used # to scale the model of the machine # most of these scale factors are arbitrary, to give # a nicely proportioned machine.  If you know specifics # for the machine you are modeling, feel free to change # these numbers tool_len = math.sqrt(d5*d5+d6*d6)    # don't change tool_dia = tool_len / 6.0 # diameters of the arms l1_dia = d2 / 5.0 l2_dia = d4 / 5.0 l3_dia = l2_dia * 0.8 # diameters of the "lumps" at the joints j0_dia = l1_dia * 1.5 j1_dia = max(l1_dia * 1.25, l2_dia * 1.5) j2_dia = l2_dia * 1.25 # other dims j0_hi = l1_dia * 1.2 j1_hi1 = l1_dia * 1.1 j1_hi2 = l2_dia * 1.2 j2_hi = l2_dia * 1.3 # don't change these tool_angle = math.degrees(math.atan2(d6,d5)) tool_radius = tool_dia / 2.0 l1_rad = l1_dia / 2.0 l2_rad = l2_dia / 2.0 l3_len = j3max + j2_hi * 0.7 l3_rad = l3_dia / 2.0 j0_hi = j0_hi / 2.0 j0_rad = j0_dia / 2.0 j1_hi1 = j1_hi1 / 2.0 j1_hi2 = j1_hi2 / 2.0 j1_rad = j1_dia / 2.0 j2_hi = j2_hi / 2.0 j2_rad = j2_dia / 2.0 size = max(d1+d3+l3_len,d2+d4+d6) # tool - cylinder with a point, and a ball to hide the blunt back end # the origin starts out at the tool tip, and we want to capture this # "tooltip" coordinate system tooltip = Capture() tool = Collection([     tooltip,     Sphere(0.0, 0.0, tool_len, tool_dia),     CylinderZ(tool_len, tool_radius, tool_dia, tool_radius),     CylinderZ(tool_dia, tool_radius, 0.0, 0.0)]) # translate so origin is at base of tool, not the tip tool = Translate([tool],0.0,0.0,-tool_len)     # the tool might not be pointing straight down tool = Rotate([tool],tool_angle,0.0,-1.0,0.0) # make joint 3 rotate tool = HalRotate([tool],c,"joint.3.pos-fb", 1, 0, 0, 1, direct = 1) link3 = CylinderZ(0.0, l3_rad, l3_len, l3_rad) # attach tool to end link3 = Collection([tool,link3]) # make joint 2 go up and down link3 = HalTranslate([link3], c, "joint.2.pos-fb", 0, 0, MODEL_SCALING, direct=1) # outer arm # start with link3 and the cylinder it slides in link2 = Collection([     link3,     CylinderZ(-j2_hi, j2_rad, j2_hi, j2_rad)]) # move to end of arm link2 = Translate([link2], d4, 0.0, 0.0) # add the arm itself link2 = Collection([     link2,     CylinderX(d4, l2_rad, 1.5*j1_rad, l2_rad)]) # the joint gets interesting, because link2 can be above or below link1 if d3 > 0:     flip = 1 else:     flip = -1 # add the joint link2 = Collection([     link2,     Box(1.5*j1_rad, -0.9*j1_rad, -j1_hi2, 1.15*j1_rad, 0.9*j1_rad, j1_hi2),     Box(1.15*j1_rad, -0.9*j1_rad, -0.4*d3, 0.0, 0.9*j1_rad, flip*j1_hi2),     CylinderZ(-0.4*d3, j1_rad, flip*1.2*j1_hi2, j1_rad)]) # make the joint work link2 = HalRotate([link2],c,"joint.1.pos-fb", 1, 0, 0, 1, direct = 1) # inner arm # the outer arm and the joint link1 = Collection([     Translate([link2],0.0,0.0,d3),     Box(-1.5*j1_rad, -0.9*j1_rad, -j1_hi1, -1.15*j1_rad, 0.9*j1_rad, j1_hi1),     Box(-1.15*j1_rad, -0.9*j1_rad, 0.4*d3, 0.0, 0.9*j1_rad, -flip*j1_hi1),     CylinderZ(0.4*d3, j1_rad, flip*-1.2*j1_hi1, j1_rad),     CylinderZ(0.6*d3, 0.8*j1_rad, 0.4*d3, 0.8*j1_rad)]) # move to end of arm link1 = Translate([link1], d2, 0.0, 0.0) # add the arm itself, and the inner joint link1 = Collection([     link1,     CylinderX(d2-1.5*j1_rad, l1_rad, 1.5*j0_rad, l1_rad),     Box(1.5*j0_rad, -0.9*j0_rad, -j0_hi, 0.0, 0.9*j0_rad, j0_hi),     CylinderZ(-1.2*j0_hi, j0_rad, 1.2*j0_hi, j0_rad)]) # make the joint work link1 = HalRotate([link1],c,"joint.0.pos-fb", 1, 0, 0, 1, direct = 1) link1 = Color([1, .5, .5, .5],[link1]) #stationary base link0 = Collection([     CylinderZ(d1-j0_hi, 0.8*j0_rad, d1-1.5*j0_hi, 0.8*j0_rad),     CylinderZ(d1-1.5*j0_hi, 0.8*j0_rad, 0.07*d1, 1.3*j0_rad),     CylinderZ(0.07*d1, 2.0*j0_rad, 0.0, 2.0*j0_rad)]) # slap the arm on top link0 = Collection([     link0,     Translate([link1],0,0,d1)]) # add a floor floor = Box(-0.5*size,-0.5*size,-0.02*size,0.5*size,0.5*size,0.0) # and a table for the workpiece - define in workpiece coords reach = d2+d4-d6 table_height = d1+d3-j3max-d5 work = Capture() table = Collection([     work,     Box(-0.35*reach,-0.5*reach, -0.1*d1, 0.35*reach, 0.5*reach, 0.0)]) # make the table moveable (tilting) #table = HalRotate([table],c,"joint.4.pos-fb", 1, 0, 1, 0, direct = 1) #table = HalRotate([table],c,"joint.5.pos-fb", 1, 1, 0, 0, direct = 1) # put the table into its proper place table = Translate([table],0.5*reach,0.0,table_height) model = Collection([link0, floor, table]) class Window(QWidget):     def __init__(self):         super(Window, self).__init__()         self.glWidget = GLWidget()         v = self.glWidget         v.set_latitudelimits(-360, 360)         size = max(d1+d3+l3_len,d2+d4+d6)         world = Capture()         #main(model, tooltip, work, size)         #v.model = Collection([model, tooltip, work, size])         v.model = Collection([model, world])         v.distance = size * 3         v.near = size * 0.01         v.far = size * 10.0         v.tool2view = tooltip         v.world2view = world         v.work2view = work         mainLayout = QHBoxLayout()         mainLayout.addWidget(self.glWidget)         self.setLayout(mainLayout) # but it you call this directly it should work too if __name__ == '__main__':     from PyQt5.QtWidgets import (QApplication, QWidget)     app = QApplication(sys.argv)     window = Window()     window.show()     sys.exit(app.exec_())