Difference between revisions of "Gripper Example"

Jump to: navigation , search
(Key Bindings)
 
(30 intermediate revisions by 3 users not shown)
Line 1: Line 1:
__NOTOC__
 
 
<div class="title-block">
 
<div class="title-block">
  
Line 6: Line 5:
 
</div>
 
</div>
  
<div class="content-block">
+
{{TOClimit|limit=2}}
 
 
== Code Walkthrough ==
 
<span style="font-size:120%;">[[Gripper Joystick Example - Code Walkthrough | Gripper Joystick Example - Code Walkthrough]]</span>
 
 
 
<span style="font-size:120%;">[[Gripper Keyboard Example - Code Walkthrough | Gripper Keyboard Example - Code Walkthrough]]</span>
 
 
 
</div>
 
  
 
<div class="content-block">
 
<div class="content-block">
Line 19: Line 11:
 
== Overview ==
 
== 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.
+
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 [https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_examples/scripts/gripper_joystick.py this link for gripper_joystick] and [https://github.com/RethinkRobotics/intera_sdk/blob/master/intera_examples/scripts/gripper_keyboard.py this link for gripper_keyboard].
  
 
</div>
 
</div>
 
  
 
<div class="content-block">
 
<div class="content-block">
Line 33: Line 24:
 
$ rosrun intera_interface robot_enable.py
 
$ rosrun intera_interface robot_enable.py
 
</syntaxhighlight>
 
</syntaxhighlight>
 
  
 
Start gripper control from an RSDK terminal session:
 
Start gripper control from an RSDK terminal session:
  
Gripper keyboard example:
+
Gripper keyboard Example:
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples gripper_keyboard.py
 
$ rosrun intera_examples gripper_keyboard.py
 +
</syntaxhighlight>
 +
 +
Gripper Joystick Example:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples gripper_joystick.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 
IMPORTANT: You will have to calibrate gripper before using any of the other commands using C/c commands:
 
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
+
Once calibrated, future calibrate commands will not do anything unless you send a 'reboot' first.
  
 
</div>
 
</div>
Line 55: Line 50:
  
 
For gripper keyboard example:
 
For gripper keyboard example:
key: description
+
<syntaxhighlight lang="bash" enclose="div">
'r': "reboot"
+
key: description,
'c': "calibrate"
+
'r': "reboot",
'q': "close"
+
'c': "calibrate",
'o': "open"
+
'q': "close",
'+': "set 100% velocity"
+
'o': "open",
'-': "set 30% velocity"
+
'+': "set 100% velocity",
's': "stop"
+
'-': "set 30% velocity",
'h': "decrease holding force"
+
's': "stop",
'j': "increase holding force"
+
'h': "decrease holding force",
'u': "decrease position"
+
'j': "increase holding force",
'i': "increase position"
+
'u': "decrease position",
 +
'i': "increase position",
 +
</syntaxhighlight>
 +
 
  
 
For gripper joystick example:
 
For gripper joystick example:
command: description
+
<syntaxhighlight lang="bash" enclose="div">
'btnLeft': "reboot"
+
command: description,
'btnUp': "calibrate"
+
'btnLeft'     : "reboot",
'leftTrigger': "close"
+
'btnUp'       : "calibrate",
'leftTrigger': "open (release)"
+
'leftTrigger' : "close",
'leftBumper': "stop"
+
'leftTrigger' : "open (release)",
'leftStickHorz': "decrease position by step when stick value less than 0, increase position by step when stick value larger than 0"
+
'leftBumper'   : "stop",
'leftStickVert': "decrease holding force by step when stick value less than 0, increase holding force by step when stick value larger than 0"
+
'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"
 
'function1' or 'function2': "help"
 +
</syntaxhighlight>
 +
  
 
You can monitor the changes you are making using the following rostopic which you can monitor from a different shell:
 
You can monitor the changes you are making using the following rostopic which you can monitor from a different shell:
Line 83: Line 84:
 
$ rostopic echo /robot/end_effector/right_gripper/command
 
$ rostopic echo /robot/end_effector/right_gripper/command
 
</syntaxhighlight>
 
</syntaxhighlight>
 +
 +
</div>
 +
 +
<div class="content-block">
 +
 +
== Joystick Control ==
 +
 +
To use the example gripper program using a joystick game controller to control the gripper:
 +
 +
First ensure that joy drivers are installed.
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rospack find joy
 +
</syntaxhighlight>
 +
 +
If not run:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ sudo apt-get install ros-indigo-joystick-drivers
 +
</syntaxhighlight>
 +
 +
To run the example:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ roslaunch intera_examples gripper_joystick.launch joystick:=<joystick_type>
 +
</syntaxhighlight>
 +
 +
Where <code>joystick_type</code> 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: [http://www.ros.org/wiki/ps3joy/Tutorials/PairingJoystickAndBluetoothDongle 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 <code>ctrl-c</code> 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.
 +
 +
[[File:Joystick.jpg|700px]]
 +
 +
{| class="wikitable"
 +
|-
 +
! scope="col"|Buttons
 +
! scope="col"|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 <code><current joints> +1</code> 
 +
|-
 +
| ''<Any Keyboard key>'' || Quit
 +
|}
 +
 +
{| class="wikitable"
 +
|-
 +
! scope="col"|Stick Axes
 +
! scope="col"|Action
 +
|-
 +
| Left Stick Horizontal    || right:  increase/decrease <code><current joint 1></code> (j0)
 +
|-
 +
| Left Stick Vertical    || right:  increase/decrease <code><current joint 2></code> (j1)
 +
|}
 +
 +
</div>
 +
 +
<div class="content-block">
 +
 +
== Gripper Joystick Example Code Walkthrough ==
 +
 +
This example demonstrates the usage of the gripper control via intera interface. The <code>main()</code> function invokes the <code>map_joystick()</code> 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.
 +
 +
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import argparse
 +
 +
import rospy
 +
 +
import intera_interface
 +
import intera_external_devices
 +
</syntaxhighlight>
 +
 +
This imports the <code>intera_interface</code> for accessing the <code>limb</code> and the <code>gripper</code> class. The <code>intera_external_devices</code> is imported to use its <code>getch</code> function that captures the key presses on the joystick.
 +
 +
<syntaxhighlight lang="python" line start="41" enclose="div">
 +
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
 +
</syntaxhighlight>
 +
 +
The <code>init_state</code> variable captures the current state of the robot. The gripper instance class, <code>gripper</code>, is created, if the gripper is not attached, a logerr message will show up.
 +
 +
<syntaxhighlight lang="python" line start="56" enclose="div">
 +
    def clean_shutdown():
 +
        print("\nExiting example...")
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
 +
</syntaxhighlight>
 +
 +
On shutdown request, Sawyer's state is sent back to its initial state.
 +
 +
<syntaxhighlight lang="python" line start="63" enclose="div">
 +
    # 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
 +
</syntaxhighlight>
 +
 +
Setup the gripper dead zone and joystick abbreviations.
 +
 +
<syntaxhighlight lang="python" line start="72" enclose="div">
 +
    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))
 +
</syntaxhighlight>
 +
 +
The <code>print_help</code> function print helpful commands and docs.
 +
 +
<syntaxhighlight lang="python" line start="80" enclose="div">
 +
    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)
 +
</syntaxhighlight>
 +
 +
Setup gripper position and holding force.
 +
 +
<syntaxhighlight lang="python" line start="88" enclose="div">
 +
    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)
 +
</syntaxhighlight>
 +
 +
The bindings is a dictionary that holds the set of characters in the joystick and their corresponding gripper functions.
 +
 +
<syntaxhighlight lang="python" line start="110" enclose="div">
 +
    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.")
 +
</syntaxhighlight>
 +
 +
The while loop iterates till the <code>Esc</code> or <code>ctrl-c</code> is inputted or ros-shutdown signal is given. If <code>Esc</code> or <code>ctrl-c</code> is given then shutdown signal is sent.
 +
 +
<syntaxhighlight lang="python" line start="125" enclose="div">
 +
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:])
 +
</syntaxhighlight>
 +
 +
Initialized the node and the joystick argument needs to be provided.
 +
 +
<syntaxhighlight lang="python" line start="125" enclose="div">
 +
    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__':
 +
</syntaxhighlight>
 +
 +
Joystick device selection.
 +
 +
</div>
 +
 +
<div class="content-block">
 +
 +
== Gripper Keyboard Example Code Walkthrough ==
 +
 +
This example demonstrates the usage of the gripper control via intera interface. The <code>main()</code> function invokes the <code>map_keyboard()</code> 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.
 +
 +
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import argparse
 +
 +
import rospy
 +
 +
import intera_interface
 +
import intera_external_devices
 +
from intera_interface import CHECK_VERSION
 +
</syntaxhighlight>
 +
 +
This imports the <code>intera interface</code> for accessing the limb and the gripper class. The <code>intera_external_devices</code> is imported to use its getch function that captures the key presses on the keyboard. The <code>CHECK_VERSION</code> 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.
 +
 +
<syntaxhighlight lang="python" line start="41" enclose="div">
 +
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
 +
</syntaxhighlight>
 +
 +
The <code>init_state</code> variable captures the current state of the robot. The <code>Gripper</code> instance class is created. If no gripper detect, the logerr message will show up.
 +
 +
<syntaxhighlight lang="python" line start="51" enclose="div">
 +
    def clean_shutdown():
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
        print("Exiting example.")
 +
    rospy.on_shutdown(clean_shutdown)
 +
</syntaxhighlight>
 +
 +
On shutdown request, Sawyer's state is sent back to its initial state.
 +
 +
<syntaxhighlight lang="python" line start="57" enclose="div">
 +
    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)
 +
</syntaxhighlight>
 +
 +
Setup gripper position and holding force.
 +
 +
<syntaxhighlight lang="python" line start="65" enclose="div">
 +
    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"),
 +
    }
 +
</syntaxhighlight>
 +
 +
The bindings is a dictionary that holds the set of characters in the keyboard and their corresponding gripper functions.
 +
 +
<syntaxhighlight lang="python" line start="82" enclose="div">
 +
    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.")
 +
</syntaxhighlight>
 +
 +
The while loop iterates till the <code>Esc</code> or <code>Ctrl-c</code> is inputted or ros-shutdown signal is given. If <code>Esc</code> or <code>Ctrl-c</code> is given then shutdown signal is sent.
 +
 +
<syntaxhighlight lang="python" line start="106" enclose="div">
 +
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)
 +
 +
 +
if __name__ == '__main__':
 +
</syntaxhighlight>
 +
 +
Initialized the node then call <code>map_keyboard</code> function.
  
 
</div>
 
</div>

Latest revision as of 20:21, 16 January 2017

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.

33 import argparse
34 
35 import rospy
36 
37 import intera_interface
38 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.

41 def map_joystick(joystick, limb):
42     """
43     maps joystick input to gripper commands
44 
45     @param joystick: an instance of a Joystick
46     """
47     print("Getting robot state... ")
48     rs = intera_interface.RobotEnable(CHECK_VERSION)
49     init_state = rs.state()
50     try:
51         gripper = intera_interface.Gripper(limb)
52     except ValueError:
53         rospy.logerr("Could not detect a gripper attached to the robot.")
54         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.

56     def clean_shutdown():
57         print("\nExiting example...")
58         if not init_state:
59             print("Disabling robot...")
60             rs.disable()
61     rospy.on_shutdown(clean_shutdown)

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

63     # decrease position dead_zone
64     gripper.set_dead_zone(2.5)
65 
66     # abbreviations
67     jhi = lambda s: joystick.stick_value(s) > 0
68     jlo = lambda s: joystick.stick_value(s) < 0
69     bdn = joystick.button_down
70     bup = joystick.button_up

Setup the gripper dead zone and joystick abbreviations.

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

The print_help function print helpful commands and docs.

80     def offset_position(offset):
81         current = gripper.get_position()
82         gripper.set_position(current + offset)
83 
84     def offset_holding(offset):
85         current = gripper.get_force()
86         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

 88     num_steps = 10.0
 89     bindings_list = []
 90     bindings = (
 91         #(test, command, description)
 92         ((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"),
 93         ((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"),
 94         ((bdn, ['leftTrigger']), (gripper.close, []), "close"),
 95         ((bup, ['leftTrigger']), (gripper.open, []), "open (release)"),
 96         ((bdn, ['leftBumper']), (gripper.stop, []), "stop"),
 97         ((jlo, ['leftStickHorz']), (offset_position, [-(gripper.MAX_POSITION / num_steps)]),
 98                                     "decrease position"),
 99         ((jhi, ['leftStickHorz']), (offset_position, [gripper.MAX_POSITION / num_steps]),
100                                      "increase position"),
101         ((jlo, ['leftStickVert']), (offset_holding, [-(gripper.MAX_FORCE / num_steps)]),
102                                     "decrease holding force"),
103         ((jhi, ['leftStickVert']), (offset_holding, [gripper.MAX_FORCE / num_steps]),
104                                     "increase holding force"),
105         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
106         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
107     )
108     bindings_list.append(bindings)

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

110     rospy.loginfo("Enabling robot...")
111     rs.enable()
112     rate = rospy.Rate(100)
113     print_help(bindings_list)
114     print("Press <Start> button for help; Ctrl-C to stop...")
115     while not rospy.is_shutdown():
116         # test each joystick condition and call binding cmd if true
117         for (test, cmd, doc) in bindings:
118             if test[0](*test[1]):
119                 cmd[0](*cmd[1])
120                 print(doc)
121         rate.sleep()
122     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.

125 def main():
126     """RSDK Gripper Example: Joystick Control
127 
128     Use a game controller to control the grippers.
129 
130     Attach a game controller to your dev machine and run this
131     example along with the ROS joy_node to control gripper
132     using the joysticks and buttons. Be sure to provide
133     the *joystick* type you are using as an argument to setup
134     appropriate key mappings.
135 
136     Uses the intera_interface.Gripper class and the helper classes
137     in intera_external_devices.Joystick.
138     """
139     epilog = """
140 See help inside the example with the "Start" button for controller
141 key bindings.
142     """
143     rp = intera_interface.RobotParams()
144     valid_limbs = rp.get_limb_names()
145     if not valid_limbs:
146         rp.log_message(("Cannot detect any limb parameters on this robot. "
147                         "Exiting."), "ERROR")
148         return
149     arg_fmt = argparse.RawDescriptionHelpFormatter
150     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
151                                      description=main.__doc__,
152                                      epilog=epilog)
153     required = parser.add_argument_group('required arguments')
154     required.add_argument(
155         '-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'],
156         help='specify the type of joystick to use'
157     )
158     parser.add_argument(
159         "-l", "--limb", dest="limb", default=valid_limbs[0],
160         choices=valid_limbs,
161         help="Limb on which to run the gripper joystick example"
162     )
163     args = parser.parse_args(rospy.myargv()[1:])

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

125     joystick = None
126     if args.joystick == 'xbox':
127         joystick = intera_external_devices.joystick.XboxController()
128     elif args.joystick == 'logitech':
129         joystick = intera_external_devices.joystick.LogitechController()
130     elif args.joystick == 'ps3':
131         joystick = intera_external_devices.joystick.PS3Controller()
132     else:
133         # Should never reach this case with proper argparse usage
134         parser.error("Unsupported joystick type '%s'" % (args.joystick))
135 
136     print("Initializing node... ")
137     rospy.init_node("sdk_gripper_joystick")
138 
139     map_joystick(joystick, args.limb)
140 
141 
142 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.

33 import argparse
34 
35 import rospy
36 
37 import intera_interface
38 import intera_external_devices
39 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.

41 def map_keyboard(limb):
42     # initialize interfaces
43     print("Getting robot state...")
44     rs = intera_interface.RobotEnable(CHECK_VERSION)
45     init_state = rs.state()
46     try:
47         gripper = intera_interface.Gripper(limb)
48     except ValueError:
49         rospy.logerr("Could not detect a gripper attached to the robot.")
50         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.

51     def clean_shutdown():
52         if not init_state:
53             print("Disabling robot...")
54             rs.disable()
55         print("Exiting example.")
56     rospy.on_shutdown(clean_shutdown)

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

57     def offset_position(offset):
58         current = gripper.get_position()
59         gripper.set_position(current + offset)
60 
61     def offset_holding(offset):
62         current = gripper.get_force()
63         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

65     num_steps = 10.0
66     thirty_percent_velocity = 0.3*(gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY
67     bindings = {
68     #   key: (function, args, description)
69         'r': (gripper.reboot, [], "reboot"),
70         'c': (gripper.calibrate, [], "calibrate"),
71         'q': (gripper.close, [], "close"),
72         'o': (gripper.open, [], "open"),
73         '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"),
74         '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"),
75         's': (gripper.stop, [], "stop"),
76         'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"),
77         'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"),
78         'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"),
79         'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"),
80     }

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

 82     done = False
 83     rospy.loginfo("Enabling robot...")
 84     rs.enable()
 85     print("Controlling grippers. Press ? for help, Esc to quit.")
 86     while not done and not rospy.is_shutdown():
 87         c = intera_external_devices.getch()
 88         if c:
 89             if c in ['\x1b', '\x03']:
 90                 done = True
 91             elif c in bindings:
 92                 cmd = bindings[c]
 93                 cmd[0](*cmd[1])
 94                 print("command: %s" % (cmd[2],))
 95             else:
 96                 print("key bindings: ")
 97                 print("  Esc: Quit")
 98                 print("  ?: Help")
 99                 for key, val in sorted(bindings.items(),
100                                        key=lambda x: x[1][2]):
101                     print("  %s: %s" % (key, val[2]))
102     # force shutdown call if caught by key handler
103     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.

106 def main():
107     """RSDK Gripper Example: Keyboard Control
108 
109     Use your dev machine's keyboard to control and configure grippers.
110 
111     Run this example to command various gripper movements while
112     adjusting gripper parameters, including calibration, velocity,
113     and force. Uses the intera_interface.Gripper class and the
114     helper function, intera_external_devices.getch.
115     """
116     epilog = """
117 See help inside the example with the '?' key for key bindings.
118     """
119     rp = intera_interface.RobotParams()
120     valid_limbs = rp.get_limb_names()
121     if not valid_limbs:
122         rp.log_message(("Cannot detect any limb parameters on this robot. "
123                         "Exiting."), "ERROR")
124         return
125     arg_fmt = argparse.RawDescriptionHelpFormatter
126     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
127                                      description=main.__doc__,
128                                      epilog=epilog)
129     parser.add_argument(
130         "-l", "--limb", dest="limb", default=valid_limbs[0],
131         choices=valid_limbs,
132         help="Limb on which to run the gripper keyboard example"
133     )
134     args = parser.parse_args(rospy.myargv()[1:])
135 
136     print("Initializing node... ")
137     rospy.init_node("sdk_gripper_keyboard")
138 
139     map_keyboard(args.limb)
140 
141 
142 if __name__ == '__main__':

Initialized the node then call map_keyboard function.