touchbotII/manipulate_fingertips.py

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

import sys

from touchbotII import Touchbot

""" This is a gesture for Touchbot II to programmatically control which
fingertips the robot has.

This script will mainly be used by the automated firmware test.  In general,
if you want to change the fingertips it will be easier and safer to manipulate
them by hand.  If this is an option, please do it.  Running this script can
cause PERMANENT DAMAGE to the robot if you are not careful, please make sure
you know the state of the robot and exactly what you are doing before
running this script.

Specifically, you need to make sure that you do not tell the robot to put large
fingertips back into a small nest position, or try to pick up new fingertips
when you already have some.  Other problems can arise if this is used when the
robot's hand is behind another object (such as the nest itself) and does not
have a clear line of travel.  Always make sure you know which fingertips are
where and that the robot can move freely before running this gesture!

This script takes in which fingers it should be changing the tips of, the size
of the tips and if it should be picking up tips or leaving them behind.

Usage:
    manipulate_fingertips fingers size should_keep_fingertips

Examples:
    To return all four size 0 fingertips to the nest'
        python manipulate_fingertips.py 1 1 1 1 0 False'
    To pick up two size 3 fingertips'
        python manipulate_fingertips.py 0 1 0 1 3 True'
"""
NUM_RETRIES = 3

try:
    fingers = [int(arg) for arg in sys.argv[1:5]]
    size = int(sys.argv[5])
    should_keep_fingertips = bool(sys.argv[6] == 'True')
    skip_first_check = False
    if len(sys.argv) > 7 and sys.argv[7] == "--fast-check":
        skip_first_check = True
except:
    print 'Usage: python %s fingers size should_keep_fingertips' % __file__
    print 'Example:'
    print '    To return all four size 0 fingertips to the nest'
    print '    python %s 1 1 1 1 0 False' % __file__
    print '    To pick up two size 3 fingertips'
    print '    python %s.py 0 1 0 1 3 True' % __file__
    sys.exit(1)

num_fingers = sum(fingers)
if num_fingers < 1:
    print 'You must specify at least 1 finger to use'
    sys.exit(1)

# Connect to the robot and configure the profile to move quickly
bot = Touchbot()
bot.SetSpeed(Touchbot.SPEED_FAST)

# Lift the hand up to get clear of anything it may be near before moving
pos = bot.GetCurrentPosition()
pos = bot.AddSafetyClearance(pos)
error_code, _ = bot.SetCartesian(pos)
if error_code:
    print 'Error: Could not move clear of starting posisiton!'
    sys.exit(1)

# Check what we can to make sure there will not be a collision
success = False
if not skip_first_check:
    if should_keep_fingertips:
        for i in range(NUM_RETRIES):
            fingertips_present = bot.DetectFingertips()
            if not any([want and have for want, have
                        in zip(fingers, fingertips_present)]):
                success = True
                break

        if not success:
            print 'Error: there are already fingertips there!'
            sys.exit(1)
    else:
        for i in range(NUM_RETRIES):
            fingertips_present = bot.DetectFingertips()
            if all([not should_drop or have for should_drop, have
                        in zip(fingers, fingertips_present)]):
                success = True
                break
        if not success:
            print 'Error: Can not return fingertips that are not attached!'
            sys.exit(1)

# Execute the swap
success = bot.ManipulateFingertips(fingers, size, should_keep_fingertips)
if not success:
    print 'Error: Error interacting with the fingertip nest'
    sys.exit(1)

# Check what we can to make sure it worked correctly
success = False
if should_keep_fingertips:
    for i in range(NUM_RETRIES):
        fingertips_present = bot.DetectFingertips()
        if all([not want or have for want, have
                in zip(fingers, fingertips_present)]):
            success = True
            break
    if not success:
        print 'Error: Not all the fingertips were picked up!'
        sys.exit(1)
else:
    for i in range(NUM_RETRIES):
        fingertips_present = bot.DetectFingertips()
        if not any([should_drop and have for should_drop, have
                    in zip(fingers, fingertips_present)]):
            success = True
            break
    if not success:
        print 'Error: Not all the fingertips were dropped!'
        sys.exit(1)

# Lift the hand up so that it gets clear of the nest to prevent accidents
pos = bot.GetCurrentPosition()
pos = bot.AddSafetyClearance(pos)
error_code, _ = bot.SetCartesian(pos)
if error_code:
    print 'Error: Could not move clear of the nest!'
    sys.exit(1)