-
Notifications
You must be signed in to change notification settings - Fork 107
/
models.py
349 lines (289 loc) · 10.7 KB
/
models.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
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
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
# The module contains the model of a hexapod
# Use it to manipulate the pose of the hexapod
import numpy as np
from copy import deepcopy
import json
from settings import PRINT_MODEL_ON_UPDATE, PRINT_MODEL_POSE_ON_UPDATE
from .linkage import Linkage
from .ground_contact_solver import get_legs_on_ground
from .templates.pose_template import HEXAPOD_POSE
from .points import (
Point,
frame_to_align_vector_a_to_b,
frame_rotxyz,
)
# Dimensions f, s, and m
#
# |-f-|
# *---*---*--------
# / | \ |
# / | \ s
# / | \ |
# *------cog------* ---
# \ | /|
# \ | / |
# \ | / |
# *---*---* |
# | |
# |---m---|
#
# y axis
# ^
# |
# |
# ----> x axis
# cog (origin)
#
#
# Relative x-axis, for each attached linkage
#
# x2 x1
# \ /
# *---*---*
# / | \
# / | \
# / | \
# x3 --*------cog------*-- x0
# \ | /
# \ | /
# \ | /
# *---*---*
# / \
# x4 x5
#
class Hexagon:
VERTEX_NAMES = [
"right-middle",
"right-front",
"left-front",
"left-middle",
"left-back",
"right-back",
]
COXIA_AXES = [0, 45, 135, 180, 225, 315]
def __init__(self, f, m, s):
self.f = f
self.m = m
self.s = s
self.cog = Point(0, 0, 0, name="center of gravity")
self.head = Point(0, s, 0, name="head")
self.vertices = [
Point(m, 0, 0, name=Hexagon.VERTEX_NAMES[0]),
Point(f, s, 0, name=Hexagon.VERTEX_NAMES[1]),
Point(-f, s, 0, name=Hexagon.VERTEX_NAMES[2]),
Point(-m, 0, 0, name=Hexagon.VERTEX_NAMES[3]),
Point(-f, -s, 0, name=Hexagon.VERTEX_NAMES[4]),
Point(f, -s, 0, name=Hexagon.VERTEX_NAMES[5]),
]
self.all_points = self.vertices + [self.cog, self.head]
class VirtualHexapod:
LEG_COUNT = 6
def __init__(self, dimensions=None):
self.dimensions = dimensions
if dimensions is None:
self.new()
else:
f = dimensions["front"]
s = dimensions["side"]
m = dimensions["middle"]
h = dimensions["coxia"]
k = dimensions["femur"]
a = dimensions["tibia"]
self.new(f, m, s, h, k, a)
def new(self, f=0, m=0, s=0, a=0, b=0, c=0):
self.body_rotation_frame = None
self.body = Hexagon(f, m, s)
self._store_body_dimensions(f, m, s, a, b, c)
# Initialize local coordinate frame
self.x_axis = Point(1, 0, 0, name="hexapod x axis")
self.y_axis = Point(0, 1, 0, name="hexapod y axis")
self.z_axis = Point(0, 0, 1, name="hexapod z axis")
# *************************
# Store legs in neutral position
# *************************
self.legs = []
vertices, axes, names = (
self.body.vertices,
Hexagon.COXIA_AXES,
Hexagon.VERTEX_NAMES,
)
for i, point, axis, name in zip(range(6), vertices, axes, names):
linkage = Linkage(
a, b, c, coxia_axis=axis, new_origin=point, name=name, id_number=i
)
self.legs.append(linkage)
self.ground_contacts = [leg.foot_tip() for leg in self.legs]
return self
def print(self):
print_hexapod(self)
def detach_body_rotate_and_translate(self, rx, ry, rz, tx, ty, tz):
# Detaches the body of the hexapod from the legs
# then rotate and translate body as if a separate entity
frame = frame_rotxyz(rx, ry, rz)
self.body_rotation_frame = frame
for point in self.body.all_points:
point.update_point_wrt(frame)
point.move_xyz(tx, ty, tz)
self._update_local_frame(frame)
def move_xyz(self, tx, ty, tz):
# Translate hexapod in the x, y, z directions
for point in self.body.all_points:
point.move_xyz(tx, ty, tz)
for leg in self.legs:
for point in leg.all_points:
point.move_xyz(tx, ty, tz)
def update_stance(self, hip_stance, leg_stance):
pose = deepcopy(HEXAPOD_POSE)
pose[1]["coxia"] = -hip_stance # right_front
pose[2]["coxia"] = hip_stance # left_front
pose[4]["coxia"] = -hip_stance # left_back
pose[5]["coxia"] = hip_stance # right_back
for key in pose.keys():
pose[key]["femur"] = leg_stance
pose[key]["tibia"] = -leg_stance
self.update(pose)
def update(self, poses):
self.body_rotation_frame = None
# Check the possibility of hexapod twisting about z axis
might_twist = self._find_if_might_twist(poses)
# Remember old ground contacts
old_contacts = deepcopy(self.ground_contacts)
# Change each leg's pose
for _, pose in poses.items():
i = pose["id"]
self.legs[i].change_pose(pose["coxia"], pose["femur"], pose["tibia"])
# Update which legs are on the ground, The new 'normal', and height
legs, self.n_axis, height = get_legs_on_ground(self.legs)
self.ground_contacts = [leg.ground_contact() for leg in legs]
if self.n_axis is None:
raise Exception("❗❗❗Pose Unstable. COG not inside support polygon")
# tilt and shift the hexapod based on new normal
frame = frame_to_align_vector_a_to_b(self.n_axis, Point(0, 0, 1))
self.rotate_and_shift(frame, height)
self._update_local_frame(frame)
# Only twist if we computed earlier that at least three hips twisted
if might_twist:
twist_frame = find_twist_frame(old_contacts, self.ground_contacts)
self.rotate_and_shift(twist_frame, 0)
if PRINT_MODEL_POSE_ON_UPDATE:
print(json.dumps(poses, indent=4))
if PRINT_MODEL_ON_UPDATE:
self.print()
def _store_body_dimensions(self, f=0, m=0, s=0, a=0, b=0, c=0):
# coxia length, femur length, tibia length
self.linkage_dimensions = [a, b, c]
# front length, middle length, side length
self.body_dimensions = [f, m, s]
self.coxia = a
self.femur = b
self.tibia = c
self.front = f
self.mid = m
self.side = s
def rotate_and_shift(self, frame, height):
# Update each point in body
for vertex in self.body.all_points:
vertex.update_point_wrt(frame, height)
# Update each point in each leg
for leg in self.legs:
leg.update_leg_wrt(frame, height)
def _find_if_might_twist(self, poses):
# hexapod will only definitely NOT twist
# if only two of the legs (currently on the ground)
# has twisted its hips/coxia
# i.e. only 2 legs with ground contact points have changed alpha angle
# i.e. we don't care if the legs which are not on the ground twisted its hips
did_change_count = 0
for leg_point in self.ground_contacts:
# find the leg id of the ground contact point
right_or_left, front_mid_or_back, _ = leg_point.name.split("-")
leg_placement = right_or_left + "-" + front_mid_or_back
leg_id = self.body.VERTEX_NAMES.index(leg_placement)
# alpha before new pose
old_hip_angle = self.legs[leg_id].coxia_angle()
# new alpha pose
new_hip_angle = get_hip_angle(leg_id, poses)
if not np.isclose(old_hip_angle, new_hip_angle or 0):
did_change_count += 1
if did_change_count >= 3:
return True
return False
def _update_local_frame(self, frame):
# Update the x, y, z axis centered at cog of hexapod
self.x_axis.update_point_wrt(frame, 0)
self.y_axis.update_point_wrt(frame, 0)
self.z_axis.update_point_wrt(frame, 0)
def get_hip_angle(leg_id, poses):
try:
return poses[leg_id]["coxia"]
except KeyError:
try:
return poses[str(leg_id)]["coxia"]
except KeyError:
return 0
return 0
def find_twist_frame(old_ground_contacts, new_ground_contacts):
# This is the frame used to twist the model about the z axis
def _make_contact_dict(contact_list):
contact_dict = {}
for leg_point in contact_list:
name = leg_point.name
contact_dict[name] = leg_point
return contact_dict
def _twist(v1, v2):
# Note: theta is in radians
# https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
theta = np.arctan2(v2.y, v2.x) - np.arctan2(v1.y, v1.x)
# frame to rotate around z
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
return np.array(
[
[cos_theta, -sin_theta, 0, 0],
[sin_theta, cos_theta, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
]
)
# Make dictionary mapping contact point name and leg_contact_point
old_contacts = _make_contact_dict(old_ground_contacts)
new_contacts = _make_contact_dict(new_ground_contacts)
# Find at least one point that's the same
same_point_name = None
for key in old_contacts.keys():
if key in new_contacts.keys():
same_point_name = key
break
# We don't know how to rotate if we don't
# know at least one point that's contacting the ground
# before and after the movement
# so we assume that the hexapod didn't move
if same_point_name is None:
return np.eye(4)
old = old_contacts[same_point_name]
new = new_contacts[same_point_name]
# Get the projection of these points in the ground
old_vector = Point(old.x, old.y, 0)
new_vector = Point(new.x, new.y, 0)
twist_frame = _twist(new_vector, old_vector)
# ❗IMPORTANT: We are assuming that because the point
# is on the ground before and after
# They should be at the same point after movement
# I can't think of a case that contradicts this as of this moment
return twist_frame
def print_hexapod(hexapod):
print("*********************")
print("Hexapod Model")
print("*********************")
print("...Vertices")
for point in hexapod.body.all_points:
print(" ", point)
print("...legs")
for leg in hexapod.legs:
for point in leg.all_points:
print(" ", point)
print("...dimensions")
print(json.dumps(hexapod.dimensions, indent=4))
print("*********************")
print("End Hexapod Model")
print("*********************")