#!/usr/bin/python
from vismach import *
import hal
c = hal.component("test")
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("joint6", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()
floor = Collection([Box(-50,-50,-1,50,50,0)])
floor = Color([0,1,0,0],[floor])
work = Capture()
tool = Capture()
tooltip = Capture()
tool = Collection([tooltip, tool])


# joint linkage...6
# config_params['6', '1.570796326', '0.0', '0.0', '2.0', 'revolute', 'cylinder']
link6= CylinderY(0,.5 , 2.0, .5)
link6= Color([0,0,2,1], [link6])
link6 = Rotate([link6],90.0,1,0,0) 
link6= Translate([link6],0,0,2.0)
link6 = Collection([ link6,tool])
link6 = Rotate([link6], -90.0,1,0,0) 
link6= Translate([link6],0,0,-2.0)
link6 = HalRotate([link6],c,"joint6",1,1,0,0)


# joint linkage...5
# config_params['5', '1.570796326', '0.0', '0.0', '2.2', 'revolute', 'cylinder']
link5= CylinderY(2.0,.5 , 4.2, .5)
link5= Color([0,2,0,1], [link5])
link5 = Rotate([link5],90.0,1,0,0) 
link5= Translate([link5],0,0,2.2)
link5 = Collection([ link5,link6])
link5 = Rotate([link5], -90.0,1,0,0) 
link5= Translate([link5],0,0,-2.2)
link5 = HalRotate([link5],c,"joint5",1,1,0,0)


# joint linkage...4
# config_params['4', '-1.570796326', '0.0', '0.0', '0', 'revolute', 'cylinder']
link4= CylinderY(4.2,.5 , 4.2, .5)
link4= Color([2,0,0,1], [link4])
link4 = Rotate([link4],-90.0,1,0,0) 
link4 = Collection([ link4,link5])
link4 = Rotate([link4], --90.0,1,0,0) 
link4 = HalRotate([link4],c,"joint4",1,1,0,0)


# joint linkage...3
# config_params['3', '1.570796326', '0.0', '0.0', '17.05', 'revolute', 'cylinder']
link3= CylinderY(4.2,.5 , 21.25, .5)
link3= Color([0,2,2,1], [link3])
link3 = Rotate([link3],90.0,1,0,0) 
link3= Translate([link3],0,0,17.05)
link3 = Collection([ link3,link4])
link3 = Rotate([link3], -90.0,1,0,0) 
link3= Translate([link3],0,0,-17.05)
link3 = HalRotate([link3],c,"joint3",1,1,0,1)


# joint linkage...2
# config_params['2', '0', '0.0', '17', '0', 'revolute', 'cylinder']
link2= CylinderY(21.25,.5 , 21.25, .5)
link2= Color([2,0,2,1], [link2])
link2= Translate([link2],17,0,0)
link2 = Collection([ link2,link3])
link2= Translate([link2],-17,0,0)
link2 = HalRotate([link2],c,"joint2",1,1,0,0)


# joint linkage...1
# config_params['1', '1.570796326', '0.0', '0.0', '-5.5', 'revolute', 'cylinder']
link1= CylinderY(21.25,.5 , 15.75, .5)
link1= Color([2,2,0,1], [link1])
link1 = Rotate([link1],90.0,1,0,0) 
link1= Translate([link1],0,0,-5.5)
link1 = Collection([ link1,link2])
link1 = Rotate([link1], -90.0,1,0,0) 
link1= Translate([link1],0,0,--5.5)
link1 = HalRotate([link1],c,"joint1",1,0,0,1)


# joint linkage...0
# config_params['0', '0', '0.0', '0.0', '26.45', 'revolute', 'cylinder']
link0= CylinderY(15.75,.5 , 42.2, .5)
link0= Color([0,0,2,1], [link0])
link0= Translate([link0],0,0,26.45)
link0 = Collection([ link0,link1])
link0= Translate([link0],0,0,-26.45)
link0 = HalRotate([link0],c,"joint0",1,1,0,0)



machine = Collection([ link1, link0])
model = Collection([tooltip, machine, work, floor])
main(model,tooltip, work, 150)