#!/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()


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

# finger (tool)
finger1 = CylinderZ(-70, 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,xaxis,yaxis,zaxis])

# show CNC-tooltip position
tooltip = Capture()

# create toolholder
link7 = AsciiSTL(filename="link7.stl")
# translate so joint to wrist is in origin
link7 = Translate([link7],-35,-35,-100)
link7 = Collection([tooltip,link7])
link7 = Color([0.75,0.75,0.75,1],[link7])

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

# 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 = Translate([link6],830,-35,-685) 
#link6 = Translate([link6],-35,-830,-685) 
# mount link7 on it
link6 = Collection([link7, link6])
#translate it back so joint 5 rotation in origin 
link6 = Translate([link6],0,0,170)
#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)
link5 = Rotate([link5],-90,0,1,0)
# translate to mount link6 to it
link5 = Translate([link5],830,-84,-515) 
#connect to end of arm
link5 = Collection([link6, link5])
#translate all so joint 4 rotation in origin
link5 = Translate([link5],0,0,344)
#apply HAL DOF
link5 = HalRotate([link5],c,"joint4",1,0,0,1) # 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)
link4 = Rotate([link4],-90,0,1,0)
# translate to join the rest of the arm to it
link4 = Translate([link4],830,-162,-237) 
# join the arm together
link4 = Collection([link5, link4])
# translate elbow joint to origin
link4 = Translate([link4],-100,-98,88) #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],-123,-96,-729) 
# join the arm together
link3 = Collection([link4, link3])
# translate elbow joint to origin
link3 = Translate([link3],0,0,380) #joint2 @ origin
#apply HAL DOF
link3 = HalRotate([link3],c,"joint2",1,0,1,0) # shoulder

# link 2 hip
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 hip to origin
link2 = Translate([link2],-186,-260,-349)
# 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],-201,-127,-254) 

robot = Collection([link2, link1])
#translate robot base to origin 
robot = Translate([robot],0,0,239) 


#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])
main(model, tooltip, work, 1200)




