def update_hexapod_plot(alpha, beta, gamma, f, s, m, h, k, a, camera, figure):
if figure is None:
HEXAPOD = deepcopy(BASE_HEXAPOD)
HEXAPOD.update(HEXAPOD_POSE)
return BASE_PLOTTER.update(HEXAPOD_FIGURE, HEXAPOD)
virtual_hexapod = VirtualHexapod().new(f or 0, m or 0, s or 0, h or 0, k
or 0, a or 0)
POSES = deepcopy(HEXAPOD_POSE)
for k, _ in POSES.items():
POSES[k] = {
'id': k,
'name': NAMES_LEG[k],
'coxia': alpha,
'femur': beta,
'tibia': gamma,
}
virtual_hexapod.update(POSES)
if camera is not None:
figure = BASE_PLOTTER.change_camera_view(figure, json.loads(camera))
return BASE_PLOTTER.update(figure, virtual_hexapod)
def update_graph(poses_json, measurements_json, relayout_data, figure):
if figure is None:
HEXAPOD = deepcopy(BASE_HEXAPOD)
HEXAPOD.update(HEXAPOD_POSE)
return BASE_PLOTTER.update(HEXAPOD_FIGURE, HEXAPOD)
if measurements_json is None:
raise PreventUpdate
# Make base hexapod model given body measurements
measurements = json.loads(measurements_json)
virtual_hexapod = VirtualHexapod(measurements)
# Configure the pose of the hexapod given joint angles
if poses_json is not None:
try:
poses = json.loads(poses_json)
except:
print("can't parse:", poses)
virtual_hexapod.update(poses)
# Update the plot to reflect pose of hexapod
figure = BASE_PLOTTER.update(figure, virtual_hexapod)
# Use current camera view to display plot
if relayout_data and 'scene.camera' in relayout_data:
camera = relayout_data['scene.camera']
figure = BASE_PLOTTER.change_camera_view(figure, camera)
return figure
def update_patterns_page(dimensions_json, poses_json, relayout_data, figure):
dimensions = helpers.load_params(dimensions_json, "dims")
poses = helpers.load_params(poses_json, "pose")
hexapod = VirtualHexapod(dimensions)
try:
hexapod.update(poses)
except Exception as alert:
return figure, helpers.make_alert_message(alert)
BASE_PLOTTER.update(figure, hexapod)
helpers.change_camera_view(figure, relayout_data)
return figure, ""
def update_kinematics_page(dimensions_json, poses_json, relayout_data, figure):
if figure is None:
return BASE_FIGURE, ""
dimensions = helpers.load_dimensions(dimensions_json)
poses = json.loads(poses_json)
hexapod = VirtualHexapod(dimensions)
try:
hexapod.update(poses)
except Exception as alert:
return figure, helpers.make_alert_message(alert)
BASE_PLOTTER.update(figure, hexapod)
helpers.change_camera_view(figure, relayout_data)
return figure, ""
def test_shared_set_points():
points = [
Point(1, 2, 3, "a"),
Point(1, 2, 3, "b"),
Point(1, 2, 3, "c"),
Point(1, 2, 3, "d"),
]
vh = VirtualHexapod(BASE_DIMENSIONS)
update_hexapod_points(vh, 1, points)
for leg_point, point in zip(vh.legs[1].all_points, points):
assert leg_point is point
def update_inverse_page(dimensions_json, ik_parameters_json, relayout_data, figure):
dimensions = helpers.load_params(dimensions_json, "dims")
ik_parameters = helpers.load_params(ik_parameters_json, "ik")
hexapod = VirtualHexapod(dimensions)
try:
poses, hexapod = inverse_kinematics_update(hexapod, ik_parameters)
except Exception as alert:
return figure, helpers.make_alert_message(alert)
if RECOMPUTE_HEXAPOD:
try:
hexapod = recompute_hexapod(dimensions, ik_parameters, poses)
except Exception as alert:
return figure, helpers.make_alert_message(alert)
BASE_PLOTTER.update(figure, hexapod)
helpers.change_camera_view(figure, relayout_data)
return figure, helpers.make_poses_message(poses)
def recompute_hexapod(dimensions, ik_parameters, poses):
# make the hexapod with all angles = 0
# update the hexapod so that we know which given points are in contact with the ground
old_hexapod = VirtualHexapod(dimensions)
old_hexapod.update_stance(ik_parameters["hip_stance"],
ik_parameters["leg_stance"])
old_contacts = deepcopy(old_hexapod.ground_contacts)
# make a new hexapod with all angles = 0
# and update given the poses/ angles we've computed
new_hexapod = VirtualHexapod(dimensions)
new_hexapod.update(poses)
new_contacts = deepcopy(new_hexapod.ground_contacts)
# get two points that are on the ground before and after
# updating to the given poses
id1, id2 = find_two_same_leg_ids(old_contacts, new_contacts)
old_p1 = deepcopy(old_hexapod.legs[id1].ground_contact())
old_p2 = deepcopy(old_hexapod.legs[id2].ground_contact())
new_p1 = deepcopy(new_hexapod.legs[id1].ground_contact())
new_p2 = deepcopy(new_hexapod.legs[id2].ground_contact())
# we must translate and rotate the hexapod with the pose
# so that the hexapod is stepping on the old predefined ground contact points
old_vector = vector_from_to(old_p1, old_p2)
new_vector = vector_from_to(new_p1, new_p2)
translate_vector = vector_from_to(new_p1, old_p1)
_, twist_frame = find_twist_to_recompute_hexapod(new_vector, old_vector)
new_hexapod.rotate_and_shift(twist_frame, 0)
twisted_p2 = new_hexapod.legs[id2].foot_tip()
translate_vector = vector_from_to(twisted_p2, old_p2)
new_hexapod.move_xyz(translate_vector.x, translate_vector.y, 0)
might_sanity_check_points(new_p1, new_p2, old_p1, old_p2, new_vector,
old_vector)
return new_hexapod
def test_set_leg_point():
vh = VirtualHexapod(BASE_DIMENSIONS)
point = Point(1, 2, 3, "b")
vh.legs[0].p0 = Point(1, 2, 3, "b")
assert vh.legs[0].p0 == point
assert vh.legs[0].all_points[0] == point
# *---*---*
# / | \
# / | \
# / | \
# x3 --*------cog------*-- x0
# \ | /
# \ | /
# \ | /
# *---*---*
# / \
# x4 x5
NAMES_LEG = Hexagon.VERTEX_NAMES
NAMES_JOINT = Linkage.POINT_NAMES
BASE_DIMENSIONS = {
"front": 100,
"side": 100,
"middle": 100,
"coxia": 100,
"femur": 100,
"tibia": 100,
}
BASE_HEXAPOD = VirtualHexapod(BASE_DIMENSIONS)
BASE_PLOTTER = HexapodPlotter()
HEXAPOD = deepcopy(BASE_HEXAPOD)
HEXAPOD.update(HEXAPOD_POSE)
BASE_FIGURE = deepcopy(HEXAPOD_FIGURE)
BASE_PLOTTER.update(BASE_FIGURE, HEXAPOD)
