Gripper Example

Created by Rico Stodt, Modified on Mon, 14 Feb, 2022 at 2:25 PM by Rico Stodt

This page provides a detailed example of how to use gripper in SDK.

Contents
Overview
Usage
Key Bindings
Joystick Control
Gripper Joystick Example Code Walkthrough
Gripper Keyboard Example Code Walkthrough

Overview

Uses the keyboard or joystick to control Sawyer's gripper. Position, velocity, holding, and moving force can be controlled and sensed. Both logitech and xbox game controllers are supported. If you would like to follow along with the actual source code for these examples on GitHub, it can be found through this link for gripper_joystick and this link for gripper_keyboard.

Usage

The robot should always enabled after start, try the command from an SDK terminal session if the robot is not enabled:

$ rosrun intera_interface robot_enable.py

Start gripper control from an RSDK terminal session:

Gripper keyboard Example:

$ rosrun intera_examples gripper_keyboard.py

Gripper Joystick Example:

$ rosrun intera_examples gripper_joystick.py

IMPORTANT: You will have to calibrate gripper before using any of the other commands using C/c commands:

Once calibrated, future calibrate commands will not do anything unless you send a 'reboot' first.

Key Bindings

Get a list of commands by entering '?'

For gripper keyboard example:

key: description,
'r': "reboot",
'c': "calibrate",
'q': "close",
'o': "open",
'+': "set 100% velocity",
'-': "set 30% velocity",
's': "stop",
'h': "decrease holding force",
'j': "increase holding force",
'u': "decrease position",
'i': "increase position",

For gripper joystick example:

command: description,
'btnLeft'      : "reboot",
'btnUp'        : "calibrate",
'leftTrigger'  : "close",
'leftTrigger'  : "open (release)",
'leftBumper'   : "stop",
'leftStickHorz': "decrease position by step when stick value less than 0, increase position by step when stick value larger than 0",
'leftStickVert': "decrease holding force by step when stick value less than 0, increase holding force by step when stick value larger than 0",
'function1' or 'function2': "help"

You can monitor the changes you are making using the following rostopic which you can monitor from a different shell:

$ rostopic echo /robot/end_effector/right_gripper/command

Joystick Control

To use the example gripper program using a joystick game controller to control the gripper:

First ensure that joy drivers are installed.

$ rospack find joy

If not run:

$ sudo apt-get install ros-indigo-joystick-drivers

To run the example:

$ roslaunch intera_examples gripper_joystick.launch joystick:=<joystick_type>

Where joystick_type is 'xbox', 'logitech', or 'ps3'. (If using a ps3, make sure you run the node from the ROS ps3joy package in a separate sudo terminal. See instructions here: ps3joy )

NOTE: This method uses an included ROS launch file to start both the gripper example and joy_node using the roslaunch tool. You can exit the gripper example by hitting any keyboard key, however you will have to ctrl-c to also cleanup the joy_node.

NOTE: Don't forget to calibrate gripper first.

NOTE: The 'joystick left <-> robot right' mappings are not typos; they assume the user is in front of the robot when using the joystick.

Buttons	Action
Function 1 or 2 (e.g. Select/Select)	Help
Left Button (X)	right: gripper calibrate
Top Button (Y)
Left Trigger [PRESS]	right: gripper close
Left Trigger [RELEASE]	right: gripper open

Left Bumper	right: cycle `<current joints> +1`
<Any Keyboard key>	Quit

Stick Axes	Action
Left Stick Horizontal	right: increase/decrease `<current joint 1>` (j0)
Left Stick Vertical	right: increase/decrease `<current joint 2>` (j1)

Gripper Joystick Example Code Walkthrough

This example demonstrates the usage of the gripper control via intera interface. The main() function invokes the map_joystick() function. It is at this function where the joystick buttons are mapped to individual gripper actions and the commands are executed periodically.

Now, let's break down the code.

import argparse

import rospy

import intera_interface
import intera_external_devices

This imports the intera_interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its getch function that captures the key presses on the joystick.

def map_joystick(joystick, limb):
    """
    maps joystick input to gripper commands

    @param joystick: an instance of a Joystick
    """
    print("Getting robot state... ")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    try:
        gripper = intera_interface.Gripper(limb)
    except ValueError:
        rospy.logerr("Could not detect a gripper attached to the robot.")
        return

The init_state variable captures the current state of the robot. The gripper instance class, gripper, is created, if the gripper is not attached, a logerr message will show up.

    def clean_shutdown():
        print("\nExiting example...")
        if not init_state:
            print("Disabling robot...")
            rs.disable()
    rospy.on_shutdown(clean_shutdown)

On shutdown request, Sawyer's state is sent back to its initial state.

    # decrease position dead_zone
    gripper.set_dead_zone(2.5)

    # abbreviations
    jhi = lambda s: joystick.stick_value(s) > 0
    jlo = lambda s: joystick.stick_value(s) < 0
    bdn = joystick.button_down
    bup = joystick.button_up

Setup the gripper dead zone and joystick abbreviations.

    def print_help(bindings_list):
        print("Press Ctrl-C to quit.")
        for bindings in bindings_list:
            for (test, _cmd, doc) in bindings:
                if callable(doc):
                    doc = doc()
                print("%s: %s" % (str(test[1]), doc))

The print_help function print helpful commands and docs.

    def offset_position(offset):
        current = gripper.get_position()
        gripper.set_position(current + offset)

    def offset_holding(offset):
        current = gripper.get_force()
        gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

    num_steps = 10.0
    bindings_list = []
    bindings = (
        #(test, command, description)
        ((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"),
        ((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"),
        ((bdn, ['leftTrigger']), (gripper.close, []), "close"),
        ((bup, ['leftTrigger']), (gripper.open, []), "open (release)"),
        ((bdn, ['leftBumper']), (gripper.stop, []), "stop"),
        ((jlo, ['leftStickHorz']), (offset_position, [-(gripper.MAX_POSITION / num_steps)]),
                                    "decrease position"),
        ((jhi, ['leftStickHorz']), (offset_position, [gripper.MAX_POSITION / num_steps]),
                                     "increase position"),
        ((jlo, ['leftStickVert']), (offset_holding, [-(gripper.MAX_FORCE / num_steps)]),
                                    "decrease holding force"),
        ((jhi, ['leftStickVert']), (offset_holding, [gripper.MAX_FORCE / num_steps]),
                                    "increase holding force"),
        ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
        ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
    )
    bindings_list.append(bindings)

The bindings is a dictionary that holds the set of characters in the joystick and their corresponding gripper functions.

    rospy.loginfo("Enabling robot...")
    rs.enable()
    rate = rospy.Rate(100)
    print_help(bindings_list)
    print("Press <Start> button for help; Ctrl-C to stop...")
    while not rospy.is_shutdown():
        # test each joystick condition and call binding cmd if true
        for (test, cmd, doc) in bindings:
            if test[0](*test[1]):
                cmd[0](*cmd[1])
                print(doc)
        rate.sleep()
    rospy.signal_shutdown("Example finished.")

The while loop iterates till the Esc or ctrl-c is inputted or ros-shutdown signal is given. If Esc or ctrl-c is given then shutdown signal is sent.

def main():
    """RSDK Gripper Example: Joystick Control

    Use a game controller to control the grippers.

    Attach a game controller to your dev machine and run this
    example along with the ROS joy_node to control gripper
    using the joysticks and buttons. Be sure to provide
    the *joystick* type you are using as an argument to setup
    appropriate key mappings.

    Uses the intera_interface.Gripper class and the helper classes
    in intera_external_devices.Joystick.
    """
    epilog = """
See help inside the example with the "Start" button for controller
key bindings.
    """
    rp = intera_interface.RobotParams()
    valid_limbs = rp.get_limb_names()
    if not valid_limbs:
        rp.log_message(("Cannot detect any limb parameters on this robot. "
                        "Exiting."), "ERROR")
        return
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'],
        help='specify the type of joystick to use'
    )
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=valid_limbs,
        help="Limb on which to run the gripper joystick example"
    )
    args = parser.parse_args(rospy.myargv()[1:])

Initialized the node and the joystick argument needs to be provided.

    joystick = None
    if args.joystick == 'xbox':
        joystick = intera_external_devices.joystick.XboxController()
    elif args.joystick == 'logitech':
        joystick = intera_external_devices.joystick.LogitechController()
    elif args.joystick == 'ps3':
        joystick = intera_external_devices.joystick.PS3Controller()
    else:
        # Should never reach this case with proper argparse usage
        parser.error("Unsupported joystick type '%s'" % (args.joystick))

    print("Initializing node... ")
    rospy.init_node("sdk_gripper_joystick")

    map_joystick(joystick, args.limb)


if __name__ == '__main__':

Joystick device selection.

Gripper Keyboard Example Code Walkthrough

This example demonstrates the usage of the gripper control via intera interface. The main() function invokes the map_keyboard() function. It is at this function where the keyboard keys are mapped to individual gripper actions and the commands are executed periodically.

Now, let's break down the code.

import argparse

import rospy

import intera_interface
import intera_external_devices
from intera_interface import CHECK_VERSION

This imports the intera interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its getch function that captures the key presses on the keyboard. The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

def map_keyboard(limb):
    # initialize interfaces
    print("Getting robot state...")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    try:
        gripper = intera_interface.Gripper(limb)
    except ValueError:
        rospy.logerr("Could not detect a gripper attached to the robot.")
        return

The init_state variable captures the current state of the robot. The Gripper instance class is created. If no gripper detect, the logerr message will show up.

    def clean_shutdown():
        if not init_state:
            print("Disabling robot...")
            rs.disable()
        print("Exiting example.")
    rospy.on_shutdown(clean_shutdown)

On shutdown request, Sawyer's state is sent back to its initial state.

    def offset_position(offset):
        current = gripper.get_position()
        gripper.set_position(current + offset)

    def offset_holding(offset):
        current = gripper.get_force()
        gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

    num_steps = 10.0
    thirty_percent_velocity = 0.3*(gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY
    bindings = {
    #   key: (function, args, description)
        'r': (gripper.reboot, [], "reboot"),
        'c': (gripper.calibrate, [], "calibrate"),
        'q': (gripper.close, [], "close"),
        'o': (gripper.open, [], "open"),
        '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"),
        '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"),
        's': (gripper.stop, [], "stop"),
        'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"),
        'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"),
        'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"),
        'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"),
    }

The bindings is a dictionary that holds the set of characters in the keyboard and their corresponding gripper functions.

    done = False
    rospy.loginfo("Enabling robot...")
    rs.enable()
    print("Controlling grippers. Press ? for help, Esc to quit.")
    while not done and not rospy.is_shutdown():
        c = intera_external_devices.getch()
        if c:
            if c in ['\x1b', '\x03']:
                done = True
            elif c in bindings:
                cmd = bindings[c]
                cmd[0](*cmd[1])
                print("command: %s" % (cmd[2],))
            else:
                print("key bindings: ")
                print("  Esc: Quit")
                print("  ?: Help")
                for key, val in sorted(bindings.items(),
                                       key=lambda x: x[1][2]):
                    print("  %s: %s" % (key, val[2]))
    # force shutdown call if caught by key handler
    rospy.signal_shutdown("Example finished.")

The while loop iterates till the Esc or Ctrl-c is inputted or ros-shutdown signal is given. If Esc or Ctrl-c is given then shutdown signal is sent.

def main():
    """RSDK Gripper Example: Keyboard Control

    Use your dev machine's keyboard to control and configure grippers.

    Run this example to command various gripper movements while
    adjusting gripper parameters, including calibration, velocity,
    and force. Uses the intera_interface.Gripper class and the
    helper function, intera_external_devices.getch.
    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    rp = intera_interface.RobotParams()
    valid_limbs = rp.get_limb_names()
    if not valid_limbs:
        rp.log_message(("Cannot detect any limb parameters on this robot. "
                        "Exiting."), "ERROR")
        return
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=valid_limbs,
        help="Limb on which to run the gripper keyboard example"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_gripper_keyboard")

    map_keyboard(args.limb)

Initialized the node then call map_keyboard function.