Gripper Example

Jump to: navigation , search

Use gripper control as an example of controlling Sawyer's gripper.

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.

Joystick.jpg

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.

  1. import argparse
  2.  
  3. import rospy
  4.  
  5. import intera_interface
  6. 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.

  1. def map_joystick(joystick, limb):
  2.     """
  3.    maps joystick input to gripper commands
  4.  
  5.    @param joystick: an instance of a Joystick
  6.    """
  7.     print("Getting robot state... ")
  8.     rs = intera_interface.RobotEnable(CHECK_VERSION)
  9.     init_state = rs.state()
  10.     try:
  11.         gripper = intera_interface.Gripper(limb)
  12.     except ValueError:
  13.         rospy.logerr("Could not detect a gripper attached to the robot.")
  14.         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.

  1.     def clean_shutdown():
  2.         print("\nExiting example...")
  3.         if not init_state:
  4.             print("Disabling robot...")
  5.             rs.disable()
  6.     rospy.on_shutdown(clean_shutdown)

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

  1.     # decrease position dead_zone
  2.     gripper.set_dead_zone(2.5)
  3.  
  4.     # abbreviations
  5.     jhi = lambda s: joystick.stick_value(s) > 0
  6.     jlo = lambda s: joystick.stick_value(s) < 0
  7.     bdn = joystick.button_down
  8.     bup = joystick.button_up

Setup the gripper dead zone and joystick abbreviations.

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

The print_help function print helpful commands and docs.

  1.     def offset_position(offset):
  2.         current = gripper.get_position()
  3.         gripper.set_position(current + offset)
  4.  
  5.     def offset_holding(offset):
  6.         current = gripper.get_force()
  7.         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

  1.     num_steps = 10.0
  2.     bindings_list = []
  3.     bindings = (
  4.         #(test, command, description)
  5.         ((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"),
  6.         ((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"),
  7.         ((bdn, ['leftTrigger']), (gripper.close, []), "close"),
  8.         ((bup, ['leftTrigger']), (gripper.open, []), "open (release)"),
  9.         ((bdn, ['leftBumper']), (gripper.stop, []), "stop"),
  10.         ((jlo, ['leftStickHorz']), (offset_position, [-(gripper.MAX_POSITION / num_steps)]),
  11.                                     "decrease position"),
  12.         ((jhi, ['leftStickHorz']), (offset_position, [gripper.MAX_POSITION / num_steps]),
  13.                                      "increase position"),
  14.         ((jlo, ['leftStickVert']), (offset_holding, [-(gripper.MAX_FORCE / num_steps)]),
  15.                                     "decrease holding force"),
  16.         ((jhi, ['leftStickVert']), (offset_holding, [gripper.MAX_FORCE / num_steps]),
  17.                                     "increase holding force"),
  18.         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
  19.         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
  20.     )
  21.     bindings_list.append(bindings)

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

  1.     rospy.loginfo("Enabling robot...")
  2.     rs.enable()
  3.     rate = rospy.Rate(100)
  4.     print_help(bindings_list)
  5.     print("Press <Start> button for help; Ctrl-C to stop...")
  6.     while not rospy.is_shutdown():
  7.         # test each joystick condition and call binding cmd if true
  8.         for (test, cmd, doc) in bindings:
  9.             if test[0](*test[1]):
  10.                 cmd[0](*cmd[1])
  11.                 print(doc)
  12.         rate.sleep()
  13.     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.

  1. def main():
  2.     """RSDK Gripper Example: Joystick Control
  3.  
  4.    Use a game controller to control the grippers.
  5.  
  6.    Attach a game controller to your dev machine and run this
  7.    example along with the ROS joy_node to control gripper
  8.    using the joysticks and buttons. Be sure to provide
  9.    the *joystick* type you are using as an argument to setup
  10.    appropriate key mappings.
  11.  
  12.    Uses the intera_interface.Gripper class and the helper classes
  13.    in intera_external_devices.Joystick.
  14.    """
  15.     epilog = """
  16. See help inside the example with the "Start" button for controller
  17. key bindings.
  18.    """
  19.     rp = intera_interface.RobotParams()
  20.     valid_limbs = rp.get_limb_names()
  21.     if not valid_limbs:
  22.         rp.log_message(("Cannot detect any limb parameters on this robot. "
  23.                         "Exiting."), "ERROR")
  24.         return
  25.     arg_fmt = argparse.RawDescriptionHelpFormatter
  26.     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
  27.                                      description=main.__doc__,
  28.                                      epilog=epilog)
  29.     required = parser.add_argument_group('required arguments')
  30.     required.add_argument(
  31.         '-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'],
  32.         help='specify the type of joystick to use'
  33.     )
  34.     parser.add_argument(
  35.         "-l", "--limb", dest="limb", default=valid_limbs[0],
  36.         choices=valid_limbs,
  37.         help="Limb on which to run the gripper joystick example"
  38.     )
  39.     args = parser.parse_args(rospy.myargv()[1:])

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

  1.     joystick = None
  2.     if args.joystick == 'xbox':
  3.         joystick = intera_external_devices.joystick.XboxController()
  4.     elif args.joystick == 'logitech':
  5.         joystick = intera_external_devices.joystick.LogitechController()
  6.     elif args.joystick == 'ps3':
  7.         joystick = intera_external_devices.joystick.PS3Controller()
  8.     else:
  9.         # Should never reach this case with proper argparse usage
  10.         parser.error("Unsupported joystick type '%s'" % (args.joystick))
  11.  
  12.     print("Initializing node... ")
  13.     rospy.init_node("sdk_gripper_joystick")
  14.  
  15.     map_joystick(joystick, args.limb)
  16.  
  17.  
  18. 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.

  1. import argparse
  2.  
  3. import rospy
  4.  
  5. import intera_interface
  6. import intera_external_devices
  7. 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.

  1. def map_keyboard(limb):
  2.     # initialize interfaces
  3.     print("Getting robot state...")
  4.     rs = intera_interface.RobotEnable(CHECK_VERSION)
  5.     init_state = rs.state()
  6.     try:
  7.         gripper = intera_interface.Gripper(limb)
  8.     except ValueError:
  9.         rospy.logerr("Could not detect a gripper attached to the robot.")
  10.         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.

  1.     def clean_shutdown():
  2.         if not init_state:
  3.             print("Disabling robot...")
  4.             rs.disable()
  5.         print("Exiting example.")
  6.     rospy.on_shutdown(clean_shutdown)

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

  1.     def offset_position(offset):
  2.         current = gripper.get_position()
  3.         gripper.set_position(current + offset)
  4.  
  5.     def offset_holding(offset):
  6.         current = gripper.get_force()
  7.         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

  1.     num_steps = 10.0
  2.     thirty_percent_velocity = 0.3*(gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY
  3.     bindings = {
  4.     #   key: (function, args, description)
  5.         'r': (gripper.reboot, [], "reboot"),
  6.         'c': (gripper.calibrate, [], "calibrate"),
  7.         'q': (gripper.close, [], "close"),
  8.         'o': (gripper.open, [], "open"),
  9.         '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"),
  10.         '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"),
  11.         's': (gripper.stop, [], "stop"),
  12.         'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"),
  13.         'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"),
  14.         'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"),
  15.         'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"),
  16.     }

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

  1.     done = False
  2.     rospy.loginfo("Enabling robot...")
  3.     rs.enable()
  4.     print("Controlling grippers. Press ? for help, Esc to quit.")
  5.     while not done and not rospy.is_shutdown():
  6.         c = intera_external_devices.getch()
  7.         if c:
  8.             if c in ['\x1b', '\x03']:
  9.                 done = True
  10.             elif c in bindings:
  11.                 cmd = bindings[c]
  12.                 cmd[0](*cmd[1])
  13.                 print("command: %s" % (cmd[2],))
  14.             else:
  15.                 print("key bindings: ")
  16.                 print("  Esc: Quit")
  17.                 print("  ?: Help")
  18.                 for key, val in sorted(bindings.items(),
  19.                                        key=lambda x: x[1][2]):
  20.                     print("  %s: %s" % (key, val[2]))
  21.     # force shutdown call if caught by key handler
  22.     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.

  1. def main():
  2.     """RSDK Gripper Example: Keyboard Control
  3.  
  4.    Use your dev machine's keyboard to control and configure grippers.
  5.  
  6.    Run this example to command various gripper movements while
  7.    adjusting gripper parameters, including calibration, velocity,
  8.    and force. Uses the intera_interface.Gripper class and the
  9.    helper function, intera_external_devices.getch.
  10.    """
  11.     epilog = """
  12. See help inside the example with the '?' key for key bindings.
  13.    """
  14.     rp = intera_interface.RobotParams()
  15.     valid_limbs = rp.get_limb_names()
  16.     if not valid_limbs:
  17.         rp.log_message(("Cannot detect any limb parameters on this robot. "
  18.                         "Exiting."), "ERROR")
  19.         return
  20.     arg_fmt = argparse.RawDescriptionHelpFormatter
  21.     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
  22.                                      description=main.__doc__,
  23.                                      epilog=epilog)
  24.     parser.add_argument(
  25.         "-l", "--limb", dest="limb", default=valid_limbs[0],
  26.         choices=valid_limbs,
  27.         help="Limb on which to run the gripper keyboard example"
  28.     )
  29.     args = parser.parse_args(rospy.myargv()[1:])
  30.  
  31.     print("Initializing node... ")
  32.     rospy.init_node("sdk_gripper_keyboard")
  33.  
  34.     map_keyboard(args.limb)
  35.  
  36.  
  37. if __name__ == '__main__':

Initialized the node then call map_keyboard function.