1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
|
#!/usr/bin/python2.4
# Copyright 2007 Ben Lipkowitz
# You may distribute this software under the GNU GPL v2 or later
#
# Hexapod visualization.
# In HAL, you must link axis.* to cartesian coordinates from
# halui, because I am too lazy to implement inverse kinematics.
# This causes some mismatch between the struts and platform.
# Hopefully this can be fixed with some twiddling.
from vismach import *
import hal
import sys
for setting in sys.argv[1:]: exec setting
#compname must be the same as given in 'loadusr -W' or
#else the comp will never be ready
compname = "hexagui"
#if(randomize): compname += str(random.randrange(0,10000))
c = hal.component(compname)
#declare hal pins here
c.newpin("joint.0", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint.1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint.2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint.3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint.4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint.5", hal.HAL_FLOAT, hal.HAL_IN)
#get the tool tip position in cartesian coordinates from emc
#so we dont have to do kinematics
c.newpin("axis.0", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis.1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis.2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis.3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis.4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis.5", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()
#################################
#draw it!
minitetra = 1
#stolen from genhexkins.h
# you must change these if you are not using minitetra
# positions of base strut ends in base (world) coordinates
base_offsets = range(6)
base_offsets[0] = (-22.950, 13.250, 0)
base_offsets[1] = (22.950, 13.250, 0)
base_offsets[2] = (22.950, 13.250, 0)
base_offsets[3] = (0, -26.5, 0)
base_offsets[4] = (0, -26.5, 0)
base_offsets[5] = (-22.950, 13.250, 0)
# position of platform strut end in platform coordinate system
plat_offsets = range(6)
plat_offsets[0] = (-1, 11.5, 0)
plat_offsets[1] = (1, 11.5, 0)
plat_offsets[2] = (10.459, -4.884, 0)
plat_offsets[3] = (9.459, -6.616, 0)
plat_offsets[4] = (-9.459, -6.616, 0)
plat_offsets[5] = (-10.459, -4.884, 0)
scale = 1
tool_len = 3
plat_radius = 11.5
plat_thickness = 2
base_radius = 28
base_thickness = 3
strut_length = 21
strut_radius = 1
#not used if joint coordinates are defined
angles = [5, 115, 125, 235, 245, 355]
#provide some reference frames
world_coords = Capture()
#i shouldnt have to do this
foo = Collection([world_coords, Sphere(0,0,0,0)])
foo = Translate([foo],0,0,0)
#tool_coords starts out at the origin
tool_coords = Capture()
work_coords = Capture()
blob = CylinderZ(-tool_len, 0.3, 0, 0)
tool = Collection([tool_coords, blob])
#tool = Translate([tool],0,0,-tool_len)
base = CylinderZ(0, base_radius, base_thickness, base_radius)
platform = CylinderZ(0, plat_radius, plat_thickness, plat_radius)
#this can probably be removed
workpiece = Box(0,0,0,4,3,2)
workpiece = Collection([work_coords, workpiece])
workpiece = Translate([workpiece],0,0,base_thickness)
struts = []
base_joints = []
plat_joints = []
for i in range(6):
#the end-cap is so we can always see where the cylinder is
inner = CylinderZ(0, 0.8*strut_radius, strut_length, 0.8*strut_radius)
endcap = CylinderZ(strut_length-1,1.2*strut_radius,strut_length,1.2*strut_radius)
inner = Collection([inner, endcap])
outer = CylinderZ(0, 1*strut_radius, strut_length, 1*strut_radius)
#account for joint offset
inner = Translate([inner],0,0,-30)
# make strut extend and retract
hal_pin = "joint." + str(i)
inner = HalTranslate([inner],c,hal_pin,0,0,scale)
inner = Translate([inner],0,0,strut_length)
strut = Collection([inner, outer])
#build platform
# plat_joint_coords += [Capture()]
plat_joint_coords = Capture()
plat_joint = BoxCentered(1,1,2)
plat_joint = Collection([plat_joint, plat_joint_coords])
#put the joints at weird locations to make an octahedron
if(minitetra):
plat_joint = Rotate([plat_joint],-120*(i%2)+60*i, 0,0,1)
x,y,z = plat_offsets[i]
plat_joint = Translate([plat_joint],x,y,z)
else:
plat_joint = Translate([plat_joint], 0.8*plat_radius,0,-plat_thickness)
plat_joint = Rotate([plat_joint], angles[i]-120*(i%2)+60, 0,0,1)
plat_joints += [plat_joint]
#build base
# base_joint_coords += [Capture()]
base_joint_coords = Capture()
base_joint = BoxCentered(2,3,1.5)
base_joint = Collection([base_joint, base_joint_coords])
#put the joints at weird locations to make an octahedron
if(minitetra):
base_joint = Rotate([base_joint],-120*(i%2)+60*i, 0,0,1)
x,y,z = base_offsets[i]
base_joint = Translate([base_joint],x,y,z)
else:
base_joint = Translate([base_joint], 0.8*base_radius,0,0)
base_joint = Rotate([base_joint], angles[i], 0,0,1)
base_joints += [base_joint]
#point strut at platform - this also translates the strut to the base joint
#because i couldnt figure out how to rotate it around the base of the strut
strut = Track([strut],base_joint_coords, plat_joint_coords, world_coords)
struts += [strut]
base = Translate([base],0,0,-base_thickness)
#de-listify
struts = Collection(struts[:])
plat_joints = Collection(plat_joints[:])
base_joints = Collection(base_joints[:])
base = Collection([base, base_joints])
platform = Collection([platform,plat_joints])
platform = Translate([platform],0,0,-(plat_thickness+tool_len))
platform = Collection([tool, platform])
####
#animate it
#must rotate first or we will be rotating around the origin
platform = HalRotate([platform], c, "axis.3",1,1,0,0)
platform = HalRotate([platform], c, "axis.4",1,0,1,0)
platform = HalRotate([platform], c, "axis.5",1,0,0,1)
platform = HalTranslate([platform],c, "axis.0",1,0,0)
platform = HalTranslate([platform],c, "axis.1",0,1,0)
platform = HalTranslate([platform],c, "axis.2",0,0,1)
platform = Collection([platform])
#put struts under platform - not perfect, oh well
#this way tool tip stays at origin so no backplot lines at startup
#struts = Translate([struts],0,0,-strut_length)
base = Translate([base],0,0,-strut_length)
workpiece = Translate([workpiece],0,0,-strut_length)
#myhud = Hud()
#myhud.show("welcome!")
#myhud.debug_track = 0
model = Collection([platform, struts, base, workpiece, foo])
#main(model, tool_coords, work_coords, size=30, hud=myhud)
main(model, tool_coords, work_coords, size=30)
|
