from vismach import *
import hal
import sys
for setting in sys.argv[1:]: exec setting
compname = "tracking-test"
c = hal.component(compname)
c.newpin("joint0", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint5", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis0", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis5", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()
tool_coords = Capture()
world_coords = Capture()
foo = Collection([world_coords, Sphere(0,0,0,0)])
foo = Translate([foo],0,0,0)
blob = CylinderZ(0, 0, 3, 1)
tool = Collection([tool_coords, blob])
scale = 0.01
tool = HalRotate([tool],c,"axis3",1,1,0,0)
tool = HalRotate([tool],c,"axis4",1,0,1,0)
tool = HalRotate([tool],c,"axis5",1,0,0,1)
tool = HalTranslate([tool],c,"axis0",scale,0,0)
tool = HalTranslate([tool],c,"axis1",0,-scale,0)
tool = HalTranslate([tool],c,"axis2",0,0,-10*scale)
tracker = CylinderZ(0,0.1,5,0.1)
tracker = Track([tracker], world_coords, tool_coords, world_coords)
workpiece = Box(0,0,0,0,0,0)
work_coords = Capture()
workpiece = Collection([work_coords, workpiece])
inner = CylinderZ(0, 1, 10, 1)
size=20
floor = Box(-0.5*size,-0.5*size,-0.02*size,0.5*size,0.5*size,0.0)
myhud = Hud()
myhud.debug_track = 1
myhud.show("right-click to reset. scroll for Z")
model = Collection([ tool, floor, workpiece, tracker, foo])
main(model, tool_coords, work_coords, size, myhud)