| # Copyright (c) 2013 The Chromium OS Authors. All rights reserved. |
| # Use of this source code is governed by a BSD-style license that can be |
| # found in the LICENSE file. |
| |
| """ Replay a finger recording with the Touchbot II |
| |
| This script takes in a device spec (for the device the gesture |
| should be replayed on) and a path file (the list of readings that |
| were recorded originally) and reproduces that gesture with the |
| robot. |
| |
| This only supports nice, clean logs. If there are stray touches, |
| palms, more than 2 fingers, etc, in the log is WILL CAUSE PROBLEMS. |
| Make sure your logs are perfect before trying to replay them or |
| it may DAMAGE THE ROBOT PERMANENTLY. |
| |
| Usage: |
| python replay.py device_spec.p path_to_follow.py |
| |
| ie: python replay.py lumpy.p spiral.p |
| """ |
| |
| import math |
| import pickle |
| import sys |
| |
| from touchbotII import Touchbot, Device |
| |
| def distance(p1, p2): |
| x1, y1 = p1 |
| x2, y2 = p2 |
| return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5 |
| |
| try: |
| # Load the device spec |
| device = Device(sys.argv[1]) |
| # Load the path to follow |
| path = pickle.load(open(sys.argv[2], 'r')) |
| except: |
| print 'Usage: python %s device_spec.p path_to_follow.p' % __file__ |
| sys.exit(-1) |
| |
| # Connect to the robot and configure the profile |
| bot = Touchbot() |
| prof = bot.GetCurrentProfile() |
| prof.speed = Touchbot.SPEED_MEDIUM |
| prof.inRange = Touchbot.BLEND_MOVEMENTS |
| bot.SetProfileData(prof) |
| |
| last_p0 = None |
| last_p1 = None |
| last_angle = 0 |
| bot.SetFingerStates([0, 0, 0, 0]) |
| for i, positions in enumerate(path): |
| # TODO charliemooney: A better way to do this than just ignoring them |
| # There are a lot of reports and can make the movement jerky if you use |
| # all of them. This thins it out. |
| if i % 4 != 0: |
| continue |
| |
| if len(positions) == 0: |
| bot.SetFingerStates([0, 0, 0, 0]) |
| continue |
| elif len(positions) == 1: |
| p0 = last_p0 if last_p0 else (0.1, 0.1) # Dummy position |
| bot.SetFingerStates([0, 1, 0, 0]) |
| elif len(positions) == 2: |
| p0 = positions[1] |
| bot.SetFingerStates([0, 1, 0, 1]) |
| else: |
| print 'ERROR, invalid state!' |
| print positions |
| sys.exit(-1) |
| |
| p1 = positions[0] |
| |
| # Make sure the fingers don't try to swap order by checking the distances |
| if last_p0 and last_p1: |
| p0_p0_distance = distance(p0, last_p0) |
| p0_p1_distance = distance(p0, last_p1) |
| if p0_p0_distance > p0_p1_distance: |
| tmp = p0 |
| p0 = p1 |
| p1 = tmp |
| last_p0 = p0 |
| last_p1 = p1 |
| |
| # To compute the position of the hand to put the fingers at the right |
| # places, center the hand over the mid-point between the fingers, then |
| # compute the distance and angle between them and rotate the wrist to |
| # that angle and open the fingers enough to place two of the fingers |
| # in the right spots |
| # + |
| # | p0 p_mid p1 |
| # | + + + |
| # | |
| # | <-----------+-----------> |
| # | dist |
| # | |
| # +-------------------------------+ |
| |
| # p1 & p1 are the relative coordinates of the two fingers |
| # p_mid is the relative coordinates of the point in between |
| p_mid = ((p1[0] + p0[0]) / 2.0, (p1[1] + p0[1]) / 2.0) |
| # Convert to the format we can actually send to the robot |
| abs_p = device.RelativePosToAbsolutePos(p_mid) |
| |
| # Find the angle between the two finger positions (p0 and p1) |
| # This must be done using absolute coordinates, because depending on the |
| # geometry of the touchpad the angles may be slightly different for |
| # different devices. |
| abs_p0 = device.RelativePosToAbsolutePos(p0) |
| abs_p1 = device.RelativePosToAbsolutePos(p1) |
| abs_p.yaw = math.degrees(math.atan((abs_p0.y - abs_p1.y) / |
| (abs_p0.x - abs_p1.x))) |
| # The fingers are offset by 45 degrees |
| abs_p.yaw += 45 |
| |
| # Check that the angle hasn't aliased onto the opposite side |
| # and correct it if it has |
| ang_change = abs(abs_p.yaw - last_angle) |
| alias_ang_change = abs((abs_p.yaw - 180) - last_angle) |
| if ang_change > alias_ang_change: |
| abs_p.yaw = abs_p.yaw - 180 |
| last_angle = abs_p.yaw |
| |
| # Find the distance between p0 and p1 and spread the fingers |
| dist = distance((abs_p0.x, abs_p0.y), (abs_p1.x, abs_p1.y)) |
| |
| # Execute the move |
| bot.SetCartesian(abs_p, finger_distance=dist, blocking=False) |
| |
| bot.SetFingerStates([0, 0, 0, 0]) |