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
|
#!/usr/bin/env python3
# Copyright 2007 Ben Lipkowitz
# You may distribute this software under the GNU GPL v2 or later
#
# see configs/tracking-test.hal
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 the comp
#will never be ready
compname = "tracking-test"
c = hal.component(compname)
#declare hal pins here
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()
#provide some reference frames
#tool_coords starts out at the origin
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)
# add a floor
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)
|
