Gripper Cuff Control Example

Created by Rico Stodt, Modified on Thu, 10 Feb, 2022 at 3:39 PM by Rico Stodt

The gripper cuff control example demonstrates basic cuff/gripper interaction on the robot. In this example cuff buttons will be connected to gripper open /close commands: 'Circle' Button will open gripper, 'Dash' Button will close gripper, Cuff 'Squeeze' will urn on Nav lights.

Contents
Introduction
Usage
Arguments
Code Walkthrough

Introduction

This program demonstrates the usage of gripper, cuff, lights interface to control the close and open actions of the gripper and navigation light. The main() function creates an instance of the GripperConnect class which connects the gripper buttons to various callback functions. If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link for gripper cuff control example.

Usage

$ rosrun intera_examples gripper_cuff_control.py -h

Arguments

Important Arguments:

-g or --gripper : The gripper limb to control.

-n or --no-lights : Specify the argument will not trigger lights on cuff while grasping.

-v or --verbose : Specify the verbose will print the debug statements from rospy, default value is rospy.INFO

usage: gripper_cuff_control.py [-h] [-g {['right']}] [-n] [-v]

SDK Gripper Button Control Example

    Connects cuff buttons to gripper open/close commands:
        'Circle' Button    - open gripper
        'Dash' Button      - close gripper
        Cuff 'Squeeze'     - turn on Nav lights

    Run this example in the background or in another terminal
    to be able to easily control the grippers by hand while
    using the robot. Can be run in parallel with other code.
    

optional arguments:
  -h, --help            show this help message and exit
  -g {['right']}, --gripper {['right']}
                        gripper limb to control (default: both)
  -n, --no-lights       do not trigger lights on cuff grasp
  -v, --verbose         print debug statements

Code Walkthrough

Now, let's break down the code.

import argparse
import sys

import rospy

from intera_interface import (
    Gripper,
    Lights,
    Cuff,
    RobotParams,
)

This imports the intera interface for accessing the Cuff, Lights and the Gripper class.

class GripperConnect(object):
    """
    Connects wrist button presses to gripper open/close commands.
    Uses the Navigator callback feature to make callbacks to connected
    action functions when the button values change.
    """

    def __init__(self, arm, lights=True):
        """
        @type arm: str
        @param arm: arm of gripper to control
        @type lights: bool
        @param lights: if lights should activate on cuff grasp
        """
        self._arm = arm
        # inputs
        self._cuff = Cuff(limb=arm)
        # connect callback fns to signals
        self._lights = None
        if lights:
            self._lights = Lights()
            self._cuff.register_callback(self._light_action,
                                         '{0}_cuff'.format(arm))

Instances of the cuff and the lights are created.

        try:
            self._gripper = Gripper(arm)
            if not (self._gripper.is_calibrated() or
                    self._gripper.calibrate() == True):
                rospy.logerr("({0}_gripper) calibration failed.".format(
                               self._gripper.name))
                raise
            self._cuff.register_callback(self._close_action,
                                         '{0}_button_upper'.format(arm))
            self._cuff.register_callback(self._open_action,
                                         '{0}_button_lower'.format(arm))
            rospy.loginfo("{0} Cuff Control initialized...".format(
                          self._gripper.name))
        except:
            self._gripper = None
            msg = ("{0} Gripper is not connected to the robot."
                   " Running cuff-light connection only.").format(arm.capitalize())
            rospy.logwarn(msg)

If the gripper attached, create the instance of the gripper then verify if the gripper was already in a calibrated state, the cuff actions will be registered afterwards. Failed to calibrate the gripper will raise error. If the gripper is not attached, warning message will appear.

    def _open_action(self, value):
        if value and self._gripper.is_ready():
            rospy.logdebug("gripper open triggered")
            self._gripper.open()
            if self._lights:
                self._set_lights('red', False)
                self._set_lights('green', True)

The method opens the gripper and set cuff light when button sends a True value.

    def _close_action(self, value):
        if value and self._gripper.is_ready():
            rospy.logdebug("gripper close triggered")
            self._gripper.close()
            if self._lights:
                self._set_lights('green', False)
                self._set_lights('red', True)

The method closes the gripper and set cuff light when button sends a True value.

    def _light_action(self, value):
        if value:
            rospy.logdebug("cuff grasp triggered")
        else:
            rospy.logdebug("cuff release triggered")
        if self._lights:
            self._set_lights('red', False)
            self._set_lights('green', False)
            self._set_lights('blue', value)

This method assigns the boolean value that was signalled to the light interface.

    def _set_lights(self, color, value):
        self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value))
        self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color),
                                                                 on=bool(value))

Set light color according to the provided params.

def main():
    """SDK Gripper Button Control Example
    Connects cuff buttons to gripper open/close commands:
        'Circle' Button    - open gripper
        'Dash' Button      - close gripper
        Cuff 'Squeeze'     - turn on Nav lights
    Run this example in the background or in another terminal
    to be able to easily control the grippers by hand while
    using the robot. Can be run in parallel with other code.
    """
    rp = 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
    if len(valid_limbs) > 1:
        valid_limbs.append("all_limbs")
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.add_argument('-g', '--gripper', dest='gripper', default=valid_limbs[0],
                        choices=[valid_limbs],
                        help='gripper limb to control (default: both)')
    parser.add_argument('-n', '--no-lights', dest='lights',
                        action='store_false',
                        help='do not trigger lights on cuff grasp')
    parser.add_argument('-v', '--verbose', dest='verbosity',
                        action='store_const', const=rospy.DEBUG,
                        default=rospy.INFO,
                        help='print debug statements')
    args = parser.parse_args(rospy.myargv()[1:])

The gripper option, the light and verbose options are parsed from the command line arguments, as entered by the user.

    rospy.init_node('sdk_gripper_cuff_control_{0}'.format(args.gripper),
                    log_level=args.verbosity)

    arms = (args.gripper,) if args.gripper != 'all_limbs' else valid_limbs[:-1]
    grip_ctrls = [GripperConnect(arm, args.lights) for arm in arms]

    print("Press cuff buttons for gripper control. Spinning...")
    rospy.spin()
    print("Gripper Button Control Finished.")
    return 0

if __name__ == '__main__':
    sys.exit(main())

The node is initialized and an instance of the GripperConnect is created.

Gripper Cuff Control Example

Contents

Introduction

Usage

Arguments

Code Walkthrough