-
Notifications
You must be signed in to change notification settings - Fork 0
/
mechanism.py
117 lines (95 loc) · 3.67 KB
/
mechanism.py
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
import numpy as np
from geometry import transform, scale, radial_line, circle
class MechanismModel(object):
def __init__(self, disc_radius=5, arm_length=7):
# Geometry of the mechanism
self.disc_radius = disc_radius
self.arm_length = arm_length
# Kinematics of the mechanism
self.disc_angle = 0
self.arm_angle = 0
def get_disc_radius(self): return self.disc_radius
def get_arm_length(self): return self.arm_length
def get_disc_angle(self): return self.disc_angle
def get_arm_angle(self): return self.arm_angle
class MechanismDrawing(object):
def __init__(self, model):
# Use the passed-in MechanismModel
self.model = model
# Geometry data only relevant for display
# arm starts pointing up in the mechanism coords
self.arm_zero = 0
# increasing arm angle decreases angle in mechanism coords
self.arm_angle_direction = -1
# disc starts pointing up in the mechanism coords
self.disc_zero = 0
# increasing disc angle increases angle in mechanism coords
self.disc_angle_direction = -1
def draw_mechanism(self):
parts = [
self.draw_arm(),
self.draw_disc(),
]
return sum(parts, [])
def draw_arm(self):
base_radius = self.model.get_disc_radius()/10.
# offset between center of disc and pivot of arm
arm_offset = [0, -self.model.get_arm_length()]
# points in the arm head
# (converted to local coords from greg's VB code
head_pts = [
[-0.04971916, 0.94869806-1],
[-0.06975647, 0.99756405-1],
[-0.05390603, 1.02858842-1],
[-0.02722403, 1.03964362-1],
[-0.04361939, 0.99904822-1],
[-0.03046844, 0.96952136-1],
[ 0. , 0.96 -1],
[ 0.03046844, 0.96952136-1],
[ 0.04361939, 0.99904822-1],
[ 0.02722403, 1.03964362-1],
[ 0.05390603, 1.02858842-1],
[ 0.06975647, 0.99756405-1],
[ 0.04971916, 0.94869806-1],
]
# scale the points in the head to match the base, and put the head into
# the center of the disk
head_pts = transform(
head_pts,
offset=[0, self.model.get_arm_length()],
scale=10*base_radius
).tolist()
# points that make up a vertical arm
pts = [
# "base"
circle(base_radius, end=-180),
# line across base
[[-base_radius,0], [base_radius,0]],
# arm head, with body attached
[[-base_radius,0]] + head_pts + [[base_radius,0]],
]
# arm angle in world coordinates
world_arm_angle = \
self.arm_zero + self.arm_angle_direction*self.model.get_arm_angle()
transformed = [
transform(pts, offset=arm_offset, angle=world_arm_angle)
for pts in pts
]
return transformed
def draw_disc(self):
disc_radius = self.model.get_disc_radius()
# points that make up a disc with marks
# n.b. that the disc center is at the origin of the world
pts = [circle(disc_radius, resolution=50)]
# 8 radial lines, evenly spaced
pts += [
radial_line(0.9*disc_radius, disc_radius, angle=theta)
for theta in np.linspace(0,360,12)
]
# disc angle in world coordinates
world_disc_angle = self.disc_angle_direction*self.model.get_disc_angle()
transformed = [
transform(pts, angle=world_disc_angle)
for pts in pts
]
return transformed