#!/usr/bin/python
#    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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


from vismach import *
import hal

c = hal.component("melfagui")
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.newpin("lnkdx",  hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("lnkdz",  hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("grip",   hal.HAL_FLOAT, hal.HAL_IN)
c.ready()


###################

#show CNC-tooltip position
tooltip = Capture()

# finger (tool)
finger1 = CylinderZ(-100, 10, 0, 0.2)

# tool coordinates axes
xaxis = Color([1,0,0,1],[CylinderX(0,3,100,3)])
yaxis = Color([0,1,0,1],[CylinderY(0,3,100,3)])
zaxis = Color([0,0,1,1],[CylinderZ(0,3,100,3)])

# combine tool and coordinate axis
finger1 = Collection([finger1,tooltip,xaxis,yaxis,zaxis])
finger1 = Rotate([finger1],90,0,1,0)
finger1 = Translate([finger1],150,0,0)

# create toolholder
link7 = AsciiSTL(filename="link7.stl")

# translate so joint to wrist is in origin
link7 = Rotate([link7],90,0,1,0)
link7 = Translate([link7],0,-35,36)
link7 = Collection([finger1,link7])
link7 = Translate([link7],0,0,0)
link7 = Color([0.75,0.75,0.75,1],[link7])

# HAL Joint6 "Hand rotation"
link7 = HalRotate([link7],c,"joint6",1,1,0,0)

# create wrist
link6 = AsciiSTL(filename="link6.stl")
# rotate and translate it so that the joint 6 is in origin
link6 = Color([0.9,0.9,0.9,1],[link6])
link6 = Rotate([link6],90,0,0,1)
link6 = Rotate([link6],90,0,1,0)
link6 = Translate([link6],-588,-35,-829) 


# mount link7 on it
link6 = Collection([link7, link6])
#translate it back so joint 5 rotation in origin 
link6 = Translate([link6],80,0,0)
#apply HAL DOF
link6 = HalRotate([link6],c,"joint5",1,0,1,0) # wrist

# link 5, wrist
link5 = AsciiSTL(filename="link5.stl")
link5 = Color([0.9,0.9,0.9,1],[link5])
link5 = Rotate([link5],90,1,0,0)
link5 = Rotate([link5],90,0,0,1)
# translate to mount link6 to it
link5 = Translate([link5],-510,-84,-830) 

#connect to end of arm
link5 = Collection([link6, link5])
#translate all so joint 4 rotation in origin
link5 = Translate([link5],344,0,0)

#apply HAL DOF
link5 = HalRotate([link5],c,"joint4",1,1,0,0) # arm twist

# link4, arm, origin is in the joint4 location
link4 = AsciiSTL(filename="link4.stl")
link4 = Color([0.9,0.9,0.9,1],[link4])
link4 = Rotate([link4],90,1,0,0)
link4 = Rotate([link4],90,0,0,1)
# translate to join the rest of the arm to it
link4 = Translate([link4],-232,-162,-830) 
# join the arm together
link4 = Collection([link5, link4])
# translate elbow joint to origin
link4 = Translate([link4],80,-98,99) #joint3 @ origin
#apply HAL DOF
link4 = HalRotate([link4],c,"joint3",1,0,1,0) # elbow

# link 3, upper arm
link3 = AsciiSTL(filename="link3.stl")
link3 = Color([0.9,0.9,0.9,1],[link3])
link3 = Rotate([link3],90,1,0,0)
link3 = Rotate([link3],90,0,0,1)
# translate to join the rest of the arm to it
link3 = Translate([link3],-125.5,-96,-731)

# join the arm together
link3 = Collection([link4, link3])
# translate elbow joint to origin
link3 = Translate([link3],0,0,380) #joint2 @ origin
link3 = Rotate([link3],90,0,1,0)
#apply HAL DOF
link3 = HalRotate([link3],c,"joint2",1,0,1,0) # shoulder

# link 2 shoulder
link2 = AsciiSTL(filename="link2.stl")
link2 = Color([0.9,0.9,0.9,1],[link2])
link2 = Rotate([link2], 90,1,0,0)
link2 = Rotate([link2], 90,0,0,1)

#move shoulder to origin
link2 = Translate([link2],-188.5,-260,-351)
# join the arm together
link2 = Collection([link3, link2])

#translate waist joint to origin 
link2 = Translate([link2], 85,100,95) 
#apply HAL DOF
link2 = HalRotate([link2],c,"joint1",1,0,0,1) # waist

link1 = AsciiSTL(filename="link1.stl")
link1 = Color([0.9,0.9,0.9,1],[link1])
link1 = Rotate([link1],90,1,0,0)
link1 = Rotate([link1],90,0,0,1)
link1 = Translate([link1],-204.5,-127,-256) 

robot = Collection([link2, link1])

#translate robot base to origin 
robot = Translate([robot],0,0,250) 


#world coordinates
xaxis0 = Color([1,0,0,1],[CylinderX(0,5,900,5)]) 
yaxis0 = Color([0,1,0,1],[CylinderY(0,5,900,5)]) 
zaxis0 = Color([0,0,1,1],[CylinderZ(0,5,900,5)])
coordw = Collection([xaxis0,yaxis0,zaxis0])


# add a floor
floor = Box(-1500,-1500,-1,1500,1500,0.0)
floor = Color([0.5,0.5,0.5,1],[floor])
floor = Collection([floor, xaxis0, yaxis0])


#show
work = Capture()
#model = Collection([robot,floor,coordw,work])
model = Collection([robot,coordw,work])

main(model, tooltip, work, 1200)



