touchbotII/one_stationary_finger.py - chromiumos/platform/touchbot - Git at Google

 # 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.

 """ This is a gesture that draws a line with one finger while holding
 another steady in a fixed spot.

 The script takes in three points in relative coordinates.  The first specifies
 where the stationary finger should be placed.  The other two specify the start
 and end coordinates for the mobile finger.  Finally it takes a speed paramater
 between 0 and 100.

 There is one optional flag to indicate how many times to tap the stationary
 finger during the gesture.  If this is omitted, the finger will just rest on
 the pad.

 Usage:
     python one_stationary_finger.py device.p stationary_pos start_pos
            end_pos speed [--taps=n]

 Example:
     # Slowly move one finger from the top left to bottom right corner while
     # keeping a finger stationary in the bottom left
     python one_stationary_finger.py link.p 0.1 0.9 0.1 0.1 0.9 0.9 10

     # The same as above but tapping in the bottom left corner 3 times
     python one_stationary_finger.py link.p 0.1 0.9 0.1 0.1 0.9 0.9 10 --taps=3
 """

 import math
 import sys
 import time

 from collections import namedtuple

 from touchbotII import Touchbot, Device, PositionArg

 NUM_STEPS = 15
 YAW_BUFFER = 20

 try:
     device = Device(sys.argv[1])
     stationary_x, stationary_y = (float(sys.argv[2]), float(sys.argv[3]))
     start_x, start_y = (float(sys.argv[4]), float(sys.argv[5]))
     end_x, end_y = (float(sys.argv[6]), float(sys.argv[7]))
     speed = float(sys.argv[8])

     num_taps = 0
     if len(sys.argv) > 9:
         for arg in sys.argv[9:len(sys.argv)]:
             if arg.startswith('--taps='):
                 num_taps = int(arg.split('=')[1])
                 break

 except:
     print ('Usage: python %s device.p stationary_pos start_pos end_pos speed' %
            __file__)
     print 'Example: python %s link.p 0.1 0.9 0.1 0.1 0.9 0.9 10' % __file__
     sys.exit(1)

 # Connect to the robot and configure the profile
 bot = Touchbot()
 prof = bot.GetCurrentProfile()
 prof.speed = speed
 prof.straight = Touchbot.STRAIGHT_INTERPOLATION
 prof.inRange = Touchbot.BLEND_MOVEMENTS
 bot.SetProfileData(prof)

 if not (bot.IsLegalRelativeCoordinate((stationary_x, stationary_y)) and
         bot.IsLegalRelativeCoordinate((start_x, start_y)) and
         bot.IsLegalRelativeCoordinate((end_x, end_y))):
     print ('All coordinates must fall in the range of %s' %
            str(Touchbot.RELATIVE_COORDINATE_RANGE))
     sys.exit(-1)
 if not bot.IsLegalSpeed(speed):
     print 'The speed %f is not acceptable for this robot' % speed
     sys.exit(-1)
 if num_taps < 0:
     print 'The number of taps to execute (%d) must be >= 0' % num_taps
     sys.exit(-1)


 abs_stationary = device.RelativePosToAbsolutePos((stationary_x, stationary_y))

 # Split the movement up into a bunch of small steps to increase precision
 # in the middle of the gesture
 last_angle = None
 positions = []
 for step in range(NUM_STEPS):
     # Compute where the moving finger should be at this point
     percent = float(step) / float(NUM_STEPS - 1)
     moving_finger_x = percent * end_x + (1.0 - percent) * start_x
     moving_finger_y = percent * end_y + (1.0 - percent) * start_y

     # Compute the hand position required to put the fingers on these spots
     mid_x = (stationary_x + moving_finger_x) / 2.0
     mid_y = (stationary_y + moving_finger_y) / 2.0
     abs_mid = device.RelativePosToAbsolutePos((mid_x, mid_y))

     # Compute the angle the hand needs to make
     abs_stationary = device.RelativePosToAbsolutePos(
                                 (stationary_x, stationary_y))
     abs_moving = device.RelativePosToAbsolutePos(
                                 (moving_finger_x, moving_finger_y))
     dx = abs_stationary.x - abs_moving.x
     dy = abs_stationary.y - abs_moving.y
     angle = math.degrees(math.atan2(dy, dx))
     # The fingers are offset by 45 degrees
     angle += 45

     # Check to see if the angle aliased onto another configuration
     # This can cause the robot to spontaneously switch the role of the fingers
     # if we don't check for it and compensate.
     if last_angle:
         curr_change = abs(angle - last_angle)
         aliased_change_pos = abs((angle + 360) - last_angle)
         aliased_change_neg = abs((angle - 360) - last_angle)
         min_change = min([curr_change, aliased_change_pos, aliased_change_neg])
         if aliased_change_pos == min_change:
             angle = angle + 360
         elif aliased_change_neg == min_change:
             angle = angle - 360
     last_angle = angle
     abs_mid.yaw = angle

     # Find the distance between the two points to spread the fingers
     dx = abs_stationary.x - abs_moving.x
     dy = abs_stationary.y - abs_moving.y
     dist = (dx ** 2 + dy ** 2) ** 0.5

     # Store this position
     positions.append((abs_mid, dist))


 # Check to make sure the wrist won't alias around to the other side
 # If it does, determine if it can be fixed with a 180 or 360 deg offset
 min_yaw, max_yaw = bot.NonBindingYawRange(positions[0][0], YAW_BUFFER)
 chosen_yaw_shift = None
 for yaw_shift in [-360, -180, 0, 180, 360]:
     legal = True
     for abs_pos, _ in positions:
         if (abs_pos.yaw + yaw_shift > max_yaw or
             abs_pos.yaw + yaw_shift < min_yaw):
             legal = False
             break
     if legal:
         chosen_yaw_shift = yaw_shift
         break

 if chosen_yaw_shift is None:
     print 'ERROR: Unable to perform this gesture with wrapping the wrist'
     sys.exit(1)

 for i in range(len(positions)):
     positions[i][0].yaw += chosen_yaw_shift

 # Nudge the wrist into the right direction before starting.  If the wrist
 # begins the gesture way on the other side of its rotation sometimes it will
 # go the wrong direction and result in problems when it tries to over-extend.
 # By nudging the wrist closer to the starting position, this shouldn't happen.
 dist_min = abs(positions[0][0].yaw - min_yaw)
 dist_max = abs(positions[0][0].yaw - max_yaw)
 nudge_pos = bot.GetCurrentPosition()
 nudge_pos.ax_4 = bot.AX_4_MAX if dist_min > dist_max else bot.AX_4_MIN
 bot.SetAngles(nudge_pos)

 # Compute which steps (if any) the taps will be executed on
 tap_steps = []
 if num_taps > 0:
     tap_steps = [(i + 1) * len(positions) / (num_taps + 1)
                  for i in range(num_taps)]
 lifted_fingers = [1, 0, 0, 0] if chosen_yaw_shift % 360 == 0 else [0, 0, 1, 0]

 # Execute the move
 for step, (abs_pos, finger_distance) in enumerate(positions):
     bot.SetCartesian(abs_pos, finger_distance=finger_distance, blocking=False)

     # If the stationary finger should tap here, do it.  The non-blocking nature
     # of the SetCartesian command should allow this to work without issues
     if step in tap_steps:
         bot.SetFingerStates([1, 0, 1, 0])
         time.sleep(bot.MINIMUM_FINGER_EXTENSION_TIME)
         bot.SetFingerStates(lifted_fingers)

     # If this is the first or last step, block then raise/lower the fingertips
     if step == 0:
         bot.Wait()
         bot.SetFingerStates([1, 0, 1, 0] if num_taps is 0 else lifted_fingers)
     elif step == (len(positions) - 1):
         bot.Wait()
         bot.SetFingerStates([0, 0, 0, 0])
	# 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.

	""" This is a gesture that draws a line with one finger while holding
	another steady in a fixed spot.

	The script takes in three points in relative coordinates. The first specifies
	where the stationary finger should be placed. The other two specify the start
	and end coordinates for the mobile finger. Finally it takes a speed paramater
	between 0 and 100.

	There is one optional flag to indicate how many times to tap the stationary
	finger during the gesture. If this is omitted, the finger will just rest on
	the pad.

	Usage:
	python one_stationary_finger.py device.p stationary_pos start_pos
	end_pos speed [--taps=n]

	Example:
	# Slowly move one finger from the top left to bottom right corner while
	# keeping a finger stationary in the bottom left
	python one_stationary_finger.py link.p 0.1 0.9 0.1 0.1 0.9 0.9 10

	# The same as above but tapping in the bottom left corner 3 times
	python one_stationary_finger.py link.p 0.1 0.9 0.1 0.1 0.9 0.9 10 --taps=3
	"""

	import math
	import sys
	import time

	from collections import namedtuple

	from touchbotII import Touchbot, Device, PositionArg

	NUM_STEPS = 15
	YAW_BUFFER = 20

	try:
	device = Device(sys.argv[1])
	stationary_x, stationary_y = (float(sys.argv[2]), float(sys.argv[3]))
	start_x, start_y = (float(sys.argv[4]), float(sys.argv[5]))
	end_x, end_y = (float(sys.argv[6]), float(sys.argv[7]))
	speed = float(sys.argv[8])

	num_taps = 0
	if len(sys.argv) > 9:
	for arg in sys.argv[9:len(sys.argv)]:
	if arg.startswith('--taps='):
	num_taps = int(arg.split('=')[1])
	break

	except:
	print ('Usage: python %s device.p stationary_pos start_pos end_pos speed' %
	__file__)
	print 'Example: python %s link.p 0.1 0.9 0.1 0.1 0.9 0.9 10' % __file__
	sys.exit(1)

	# Connect to the robot and configure the profile
	bot = Touchbot()
	prof = bot.GetCurrentProfile()
	prof.speed = speed
	prof.straight = Touchbot.STRAIGHT_INTERPOLATION
	prof.inRange = Touchbot.BLEND_MOVEMENTS
	bot.SetProfileData(prof)

	if not (bot.IsLegalRelativeCoordinate((stationary_x, stationary_y)) and
	bot.IsLegalRelativeCoordinate((start_x, start_y)) and
	bot.IsLegalRelativeCoordinate((end_x, end_y))):
	print ('All coordinates must fall in the range of %s' %
	str(Touchbot.RELATIVE_COORDINATE_RANGE))
	sys.exit(-1)
	if not bot.IsLegalSpeed(speed):
	print 'The speed %f is not acceptable for this robot' % speed
	sys.exit(-1)
	if num_taps < 0:
	print 'The number of taps to execute (%d) must be >= 0' % num_taps
	sys.exit(-1)


	abs_stationary = device.RelativePosToAbsolutePos((stationary_x, stationary_y))

	# Split the movement up into a bunch of small steps to increase precision
	# in the middle of the gesture
	last_angle = None
	positions = []
	for step in range(NUM_STEPS):
	# Compute where the moving finger should be at this point
	percent = float(step) / float(NUM_STEPS - 1)
	moving_finger_x = percent * end_x + (1.0 - percent) * start_x
	moving_finger_y = percent * end_y + (1.0 - percent) * start_y

	# Compute the hand position required to put the fingers on these spots
	mid_x = (stationary_x + moving_finger_x) / 2.0
	mid_y = (stationary_y + moving_finger_y) / 2.0
	abs_mid = device.RelativePosToAbsolutePos((mid_x, mid_y))

	# Compute the angle the hand needs to make
	abs_stationary = device.RelativePosToAbsolutePos(
	(stationary_x, stationary_y))
	abs_moving = device.RelativePosToAbsolutePos(
	(moving_finger_x, moving_finger_y))
	dx = abs_stationary.x - abs_moving.x
	dy = abs_stationary.y - abs_moving.y
	angle = math.degrees(math.atan2(dy, dx))
	# The fingers are offset by 45 degrees
	angle += 45

	# Check to see if the angle aliased onto another configuration
	# This can cause the robot to spontaneously switch the role of the fingers
	# if we don't check for it and compensate.
	if last_angle:
	curr_change = abs(angle - last_angle)
	aliased_change_pos = abs((angle + 360) - last_angle)
	aliased_change_neg = abs((angle - 360) - last_angle)
	min_change = min([curr_change, aliased_change_pos, aliased_change_neg])
	if aliased_change_pos == min_change:
	angle = angle + 360
	elif aliased_change_neg == min_change:
	angle = angle - 360
	last_angle = angle
	abs_mid.yaw = angle

	# Find the distance between the two points to spread the fingers
	dx = abs_stationary.x - abs_moving.x
	dy = abs_stationary.y - abs_moving.y
	dist = (dx 2 + dy 2) ** 0.5

	# Store this position
	positions.append((abs_mid, dist))


	# Check to make sure the wrist won't alias around to the other side
	# If it does, determine if it can be fixed with a 180 or 360 deg offset
	min_yaw, max_yaw = bot.NonBindingYawRange(positions[0][0], YAW_BUFFER)
	chosen_yaw_shift = None
	for yaw_shift in [-360, -180, 0, 180, 360]:
	legal = True
	for abs_pos, _ in positions:
	if (abs_pos.yaw + yaw_shift > max_yaw or
	abs_pos.yaw + yaw_shift < min_yaw):
	legal = False
	break
	if legal:
	chosen_yaw_shift = yaw_shift
	break

	if chosen_yaw_shift is None:
	print 'ERROR: Unable to perform this gesture with wrapping the wrist'
	sys.exit(1)

	for i in range(len(positions)):
	positions[i][0].yaw += chosen_yaw_shift

	# Nudge the wrist into the right direction before starting. If the wrist
	# begins the gesture way on the other side of its rotation sometimes it will
	# go the wrong direction and result in problems when it tries to over-extend.
	# By nudging the wrist closer to the starting position, this shouldn't happen.
	dist_min = abs(positions[0][0].yaw - min_yaw)
	dist_max = abs(positions[0][0].yaw - max_yaw)
	nudge_pos = bot.GetCurrentPosition()
	nudge_pos.ax_4 = bot.AX_4_MAX if dist_min > dist_max else bot.AX_4_MIN
	bot.SetAngles(nudge_pos)

	# Compute which steps (if any) the taps will be executed on
	tap_steps = []
	if num_taps > 0:
	tap_steps = [(i + 1) * len(positions) / (num_taps + 1)
	for i in range(num_taps)]
	lifted_fingers = [1, 0, 0, 0] if chosen_yaw_shift % 360 == 0 else [0, 0, 1, 0]

	# Execute the move
	for step, (abs_pos, finger_distance) in enumerate(positions):
	bot.SetCartesian(abs_pos, finger_distance=finger_distance, blocking=False)

	# If the stationary finger should tap here, do it. The non-blocking nature
	# of the SetCartesian command should allow this to work without issues
	if step in tap_steps:
	bot.SetFingerStates([1, 0, 1, 0])
	time.sleep(bot.MINIMUM_FINGER_EXTENSION_TIME)
	bot.SetFingerStates(lifted_fingers)

	# If this is the first or last step, block then raise/lower the fingertips
	if step == 0:
	bot.Wait()
	bot.SetFingerStates([1, 0, 1, 0] if num_taps is 0 else lifted_fingers)
	elif step == (len(positions) - 1):
	bot.Wait()
	bot.SetFingerStates([0, 0, 0, 0])