Difference between pages "Running Examples Overview" and "Joint Position Example"

(Difference between pages)
Jump to: navigation , search
(Movement edit)
 
(Code Walkthrough 2)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''This section describes the SDK example programs. These example programs are provided as a starting point, demonstrating usage of the Python SDK packages for control of your robot.'''</span>
+
<span style="font-size:120%;">'''The joint position example(keyboard, joystick) demonstrates basic joint position control. The key or button mapped to either increasing or decreasing the angle of a particular joint on Sawyer's arm.'''</span>
  
 
</div>
 
</div>
  
 
<div class="content-block">
 
<div class="content-block">
 +
== Usage ==
  
== Prerequisites ==
+
Start the joint position keyboard example program, ex:
  
* You have completed all of the [[Installation|Installation]] instructions for setting up your robot and development machine.
+
<syntaxhighlight lang="bash" enclose="div">
* The development workstation has been connected to the robot and [[Hello Robot | connectivity has been validated]].
+
$ rosrun intera_examples joint_position_keyboard.py
 +
</syntaxhighlight>
 +
 
 +
Start the joint position joystick example program, ex:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_position_joystick.py
 +
</syntaxhighlight>
 +
 
 +
Upon startup, you will be prompted with the following:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
Initializing node...
 +
Getting robot state...  
 +
Enabling robot...
 +
[INFO] [WallTime: 1399575163.891211] Robot Enabled
 +
Controlling joints. Press ? for help, Esc to quit.
 +
</syntaxhighlight>
  
 
</div>
 
</div>
Line 17: Line 35:
 
<div class="content-block">
 
<div class="content-block">
  
== Running Examples with the SDK ==
+
== Arguments ==
  
=== Initialize ===
+
'''Important Arguments:''' 
  
[[SDK Shell | Initialize your environment]]:
+
See the joint position example available arguments on the command line by passing:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_position_keyboard.py -h
 +
</syntaxhighlight>
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ cd ~/ros_ws
+
usage: joint_position_keyboard.py [-h] [-l LIMB]
$ ./intera.sh
+
 
 +
RSDK Joint Position Example: Keyboard Control
 +
    Use your dev machine's keyboard to control joint positions.
 +
    Each key corresponds to increasing or decreasing the angle
 +
    of a joint on Sawyer's arm. The increasing and descreasing
 +
    are represented by number key and letter key next to the number.
 +
   
 +
 
 +
optional arguments:
 +
  -h, --help            show this help message and exit
 +
  -l LIMB, --limb LIMB
 +
                        Limb on which to run the joint position keyboard example
 
</syntaxhighlight>
 
</syntaxhighlight>
  
=== Run a program! ===
+
For joint position joystick example:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_position_joystick.py -h
 +
</syntaxhighlight>
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples <example_program> [arguments]
+
usage: joint_position_joystick.py [-h] [-l LIMB] [-j JOYSTICK]
</syntaxhighlight>
+
 
 +
SDK Joint Position Example: Joystick Control
 +
    Use a game controller to control the angular joint positions
 +
    of Sawyer's arms.
 +
    Attach a game controller to your dev machine and run this
 +
    example along with the ROS joy_node to control the position
 +
    of each joint in Sawyer's arm using the joystick. Be sure to
 +
    provide your *joystick* type to setup appropriate key mappings.
 +
    Each stick axis maps to a joint angle; which joints are currently
 +
    controlled can be incremented by using the mapped increment buttons.
 +
    Ex:
 +
      (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
  
Refer to the individual examples below for a complete list of arguments or run it as
+
required arguments:
 +
  -j, --joystick        specify the type of joystick to use
  
<syntaxhighlight lang="bash" enclose="div">
+
optional arguments:
$ rosrun intera_examples <example_program> -h
+
  -h, --help            show this help message and exit
 +
  -l LIMB, --limb LIMB
 +
                        Limb on which to run the joint position joystick example
 
</syntaxhighlight>
 
</syntaxhighlight>
  
to find the list of arguments.
 
  
 
</div>
 
</div>
Line 46: Line 94:
 
<div class="content-block">
 
<div class="content-block">
  
== Clone the SDK Examples ==
+
== Joint Position Keyboard Code Walkthrough ==
 +
 
 +
Now, let's break down the code.
 +
 
 +
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
"""
 +
SDK Joint Position Example: keyboard
 +
"""
 +
import argparse
 +
 
 +
import rospy
 +
 
 +
import intera_interface
 +
import intera_external_devices
 +
 
 +
from intera_interface import CHECK_VERSION
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="43" enclose="div">
 +
def map_keyboard(side):
 +
    limb = intera_interface.Limb(side)
 +
   
 +
    try:
 +
        gripper = intera_interface.Gripper(side)
 +
    except:
 +
        has_gripper = False
 +
        rospy.logerr("Could not initalize the gripper.")
 +
    else:
 +
        has_gripper = True
 +
 
 +
    joints = limb.joint_names()
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="56" enclose="div">
 +
    def set_j(limb, joint_name, delta):
 +
        current_position = limb.joint_angle(joint_name)
 +
        joint_command = {joint_name: current_position + delta}
 +
        limb.set_joint_positions(joint_command)
 +
 
 +
    def set_g(action):
 +
        if has_gripper:
 +
            if action == "close":
 +
                gripper.close()
 +
            elif action == "open":
 +
                gripper.open()
 +
            elif action == "calibrate":
 +
                gripper.calibrate()
 +
</syntaxhighlight>
  
The <code>'intera_examples'</code> locate under intera_sdk repo, put <code>'intera_sdk'</code> into your ROS development workspace (<code> ~/ros_ws/src </code>). This will allow you to dig into the examples, follow along with the code walkthroughs and base your custom code off of these examples.
+
<syntaxhighlight lang="python" line start="79" enclose="div">
 +
    bindings = {
 +
        '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"),
 +
        'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"),
 +
        '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"),
 +
        'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"),
 +
        '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"),
 +
        'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"),
 +
        '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"),
 +
        'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"),
 +
        '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"),
 +
        't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"),
 +
        '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"),
 +
        'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"),
 +
        '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"),
 +
        'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease"),
 +
        '8': (set_g, "close", side+" gripper close"),
 +
        'i': (set_g, "open", side+" gripper open"),
 +
        '9': (set_g, "calibrate", side+" gripper calibrate")
 +
    }
 +
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
<syntaxhighlight lang="python" line start="89" enclose="div">
$ cd ~/ros_ws/src
+
    done = False
$ git clone https://github.com/RethinkRobotics/intera_sdk.git
+
    print("Controlling joints. Press ? for help, Esc to quit.")
 +
    while not done and not rospy.is_shutdown():
 +
        c = intera_external_devices.getch()
 +
        if c:
 +
            #catch Esc or ctrl-c
 +
            if c in ['\x1b', '\x03']:
 +
                done = True
 +
                rospy.signal_shutdown("Example finished.")
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="98" enclose="div">
 +
            elif c in bindings:
 +
                cmd = bindings[c]
 +
                if c == '8' or c == 'i' or c == '9':
 +
                    cmd[0](cmd[1])
 +
                    print("command: %s" % (cmd[2],))
 +
                else:
 +
                    #expand binding to something like "set_j(right, 'j0', 0.1)"
 +
                    cmd[0](*cmd[1])
 +
                    print("command: %s" % (cmd[2],))
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="107" enclose="div">
 +
            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]))
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="115" enclose="div">
 +
def main():
 +
    """RSDK Joint Position Example: Keyboard Control
 +
    Use your dev machine's keyboard to control joint positions.
 +
    Each key corresponds to increasing or decreasing the angle
 +
    of a joint on Sawyer's arm. The increasing and descreasing
 +
    are represented by number key and letter key next to the number.
 +
    """
 +
    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 joint position keyboard example"
 +
    )
 +
    args = parser.parse_args(rospy.myargv()[1:])
 +
 
 +
    print("Initializing node... ")
 +
    rospy.init_node("sdk_joint_position_keyboard")
 +
    print("Getting robot state... ")
 +
    rs = intera_interface.RobotEnable(CHECK_VERSION)
 +
    init_state = rs.state().enabled
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="150" enclose="div">
 +
    def clean_shutdown():
 +
        print("\nExiting example.")
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="157" enclose="div">
 +
    rospy.loginfo("Enabling robot...")
 +
    rs.enable()
 +
    map_keyboard(args.limb)
 +
    print("Done.")
 +
 
 +
 
 +
if __name__ == '__main__':
 +
    main()
 
</syntaxhighlight>
 
</syntaxhighlight>
 +
  
 
</div>
 
</div>
  
<div class="content-block">
+
== Joint Position Joystick Code Walkthrough ==
 +
 
 +
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>
 +
 
 +
<syntaxhighlight lang="python" line start="43" enclose="div">
 +
def rotate(l):
 +
    """
 +
    Rotates a list left.
 +
    @param l: the list
 +
    """
 +
    if len(l):
 +
        v = l[0]
 +
        l[:-1] = l[1:]
 +
        l[-1] = v
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="54" enclose="div">
 +
def set_j(cmd, limb, joints, index, delta):
 +
    """
 +
    Set the selected joint to current pos + delta.
 +
    @param cmd: the joint command dictionary
 +
    @param limb: the limb to get the pos from
 +
    @param joints: a list of joint names
 +
    @param index: the index in the list of names
 +
    @param delta: delta to update the joint by
 +
    joint/index is to make this work in the bindings.
 +
    """
 +
    joint = joints[index]
 +
    cmd[joint] = delta + limb.joint_angle(joint)
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="python" line start="69" enclose="div">
 +
def map_joystick(joystick, side):
 +
    """
 +
    Maps joystick input to joint position commands.
 +
    @param joystick: an instance of a Joystick
 +
    """
 +
    limb = intera_interface.Limb(side)
 +
    gripper = None
 +
    try:
 +
        gripper = intera_interface.Gripper(side)
 +
    except:
 +
        rospy.loginfo("Could not detect a connected electric gripper.")
 +
</syntaxhighlight>
  
== SDK Examples ==
+
<syntaxhighlight lang="python" line start="82" enclose="div">
 +
    def set_g(action):
 +
        if gripper is not None:
 +
            if action == "close":
 +
                gripper.close()
 +
            elif action == "open":
 +
                gripper.open()
 +
            elif action == "calibrate":
 +
                gripper.calibrate()
 +
</syntaxhighlight>
  
=== Movement ===
+
<syntaxhighlight lang="python" line start="91" enclose="div">
 +
    limb_cmd = {}
  
[[Joint Position Waypoint Example]] - Hand-over-hand teach and recording a number of joint position waypoints. These waypoints will then be played back.
+
    #available joints
 +
    joints = limb.joint_names()
  
[[Joint Position Example]] - Joystick, keyboard control for Sawyer's arm.
+
    #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>
  
[[Joint Torque Springs Example]] - Joint torque control example applying virtual spring torques.
+
<syntaxhighlight lang="python" line start="102" 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][0]), doc))
  
[[IK Service Example|Inverse Kinematics Service Example]] - Basic use of Inverse Kinematics solver service.
+
    bindings_list = []
 +
    bindings = [
 +
        ((jlo, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, 0.1]),
 +
            lambda: "Increase " + joints[0]),
 +
        ((jhi, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, -0.1]),
 +
            lambda: "Decrease " + joints[0]),
 +
        ((jlo, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, 0.1]),
 +
            lambda: "Increase " + joints[1]),
 +
        ((jhi, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, -0.1]),
 +
            lambda: "Decrease " + joints[1]),
 +
        ((jlo, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, 0.1]),
 +
            lambda: "Increase " + joints[2]),
 +
        ((jhi, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, -0.1]),
 +
            lambda: "Decrease " + joints[2]),
 +
        ((jlo, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, 0.1]),
 +
            lambda: "Increase " + joints[3]),
 +
        ((jhi, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, -0.1]),
 +
            lambda: "Decrease " + joints[3]),
 +
        ((bdn, ['leftBumper']), (rotate, [joints]), side + ": cycle joint"),
 +
        ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
 +
        ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
 +
        ]
 +
    if gripper:
 +
        bindings.extend([
 +
            ((bdn, ['rightTrigger']), (set_g, ['close'], gripper), side + " gripper close"),
 +
            ((bup, ['rightTrigger']), (set_g, ['open'], gripper), side + " gripper open"),
 +
            ((bdn, ['btnLeft']), (set_g, ['calibrate'], gripper), "right calibrate")
 +
            ])
 +
    bindings_list.append(bindings)
  
[[Joint Trajectory Playback Example]] - Trajectory playback using the joint trajectory interface.
+
    rate = rospy.Rate(100)
 +
    print_help(bindings_list)
 +
    print("Press Ctrl-C to stop. ")
 +
    while not rospy.is_shutdown():
 +
        for (test, cmd, doc) in bindings:
 +
            if test[0](*test[1]):
 +
                cmd[0](*cmd[1])
 +
                if callable(doc):
 +
                    print(doc())
 +
                else:
 +
                    print(doc)
 +
        if len(limb_cmd):
 +
            limb.set_joint_positions(limb_cmd)
 +
            limb_cmd.clear()
 +
        rate.sleep()
 +
    return False
 +
</syntaxhighlight>
  
[[Head Movement Example]] - Simple demo moving and nodding the head.
+
<syntaxhighlight lang="python" line start="158" enclose="div">
 +
def main():
 +
    """SDK Joint Position Example: Joystick Control
 +
    Use a game controller to control the angular joint positions
 +
    of Sawyer's arms.
 +
    Attach a game controller to your dev machine and run this
 +
    example along with the ROS joy_node to control the position
 +
    of each joint in Sawyer's arm using the joystick. Be sure to
 +
    provide your *joystick* type to setup appropriate key mappings.
 +
    Each stick axis maps to a joint angle; which joints are currently
 +
    controlled can be incremented by using the mapped increment buttons.
 +
    Ex:
 +
      (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
 +
    """
 +
    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 joint position joystick example"
 +
    )
 +
    args = parser.parse_args(rospy.myargv()[1:])
  
[[Gripper Example]] - Joystick and Keyboard control for the grippers.
+
    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:
 +
        parser.error("Unsupported joystick type '%s'" % (args.joystick))
  
[[Gripper Cuff Control Example]] - Simple cuff-interaction control with Zero-G mode.
+
    print("Initializing node... ")
 +
    rospy.init_node("sdk_joint_position_joystick")
 +
    print("Getting robot state... ")
 +
    rs = intera_interface.RobotEnable(CHECK_VERSION)
 +
    init_state = rs.state().enabled
 +
</syntaxhighlight>
  
=== Robot Configuration ===
+
<syntaxhighlight lang="python" line start="217" enclose="div">
 +
    def clean_shutdown():
 +
        print("\nExiting example.")
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
  
[[URDF Configuration Example]] - A simple ROS node that shows how to add segment and joint subtrees to the robot's model.
+
    rospy.loginfo("Enabling robot...")
 +
    rs.enable()
  
=== Input and Output ===
+
    map_joystick(joystick, args.limb)
 +
    print("Done.")
  
[[Display Image Example]] - Example tool for displaying image files (png, jpeg) on the Head Screen.
 
  
[[image_view|View Cameras Example]] - Simple tool for viewing camera feed on development machine.
+
if __name__ == '__main__':
 +
    main()
 +
</syntaxhighlight>
  
[[Input Output Example|I/O Example]] - Flash the lights on the digital outputs.
 
  
 
</div>
 
</div>

Revision as of 10:44, 27 November 2016

The joint position example(keyboard, joystick) demonstrates basic joint position control. The key or button mapped to either increasing or decreasing the angle of a particular joint on Sawyer's arm.

Usage

Start the joint position keyboard example program, ex:

$ rosrun intera_examples joint_position_keyboard.py

Start the joint position joystick example program, ex:

$ rosrun intera_examples joint_position_joystick.py

Upon startup, you will be prompted with the following:

Initializing node... 
Getting robot state... 
Enabling robot... 
[INFO] [WallTime: 1399575163.891211] Robot Enabled
Controlling joints. Press ? for help, Esc to quit.

Arguments

Important Arguments:

See the joint position example available arguments on the command line by passing:

$ rosrun intera_examples joint_position_keyboard.py -h
usage: joint_position_keyboard.py [-h] [-l LIMB]

RSDK Joint Position Example: Keyboard Control
    Use your dev machine's keyboard to control joint positions.
    Each key corresponds to increasing or decreasing the angle
    of a joint on Sawyer's arm. The increasing and descreasing
    are represented by number key and letter key next to the number.
    

optional arguments:
  -h, --help            show this help message and exit
  -l LIMB, --limb LIMB
                        Limb on which to run the joint position keyboard example

For joint position joystick example:

$ rosrun intera_examples joint_position_joystick.py -h
usage: joint_position_joystick.py [-h] [-l LIMB] [-j JOYSTICK]

SDK Joint Position Example: Joystick Control
    Use a game controller to control the angular joint positions
    of Sawyer's arms.
    Attach a game controller to your dev machine and run this
    example along with the ROS joy_node to control the position
    of each joint in Sawyer's arm using the joystick. Be sure to
    provide your *joystick* type to setup appropriate key mappings.
    Each stick axis maps to a joint angle; which joints are currently
    controlled can be incremented by using the mapped increment buttons.
    Ex:
      (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)

required arguments:
  -j, --joystick        specify the type of joystick to use

optional arguments:
  -h, --help            show this help message and exit
  -l LIMB, --limb LIMB
                        Limb on which to run the joint position joystick example


Joint Position Keyboard Code Walkthrough

Now, let's break down the code.

33 """
34 SDK Joint Position Example: keyboard
35 """
36 import argparse
37 
38 import rospy
39 
40 import intera_interface
41 import intera_external_devices
42 
43 from intera_interface import CHECK_VERSION
43 def map_keyboard(side):
44     limb = intera_interface.Limb(side)
45     
46     try:
47         gripper = intera_interface.Gripper(side)
48     except:
49         has_gripper = False
50         rospy.logerr("Could not initalize the gripper.")
51     else:
52         has_gripper = True
53 
54     joints = limb.joint_names()
56     def set_j(limb, joint_name, delta):
57         current_position = limb.joint_angle(joint_name)
58         joint_command = {joint_name: current_position + delta}
59         limb.set_joint_positions(joint_command)
60 
61     def set_g(action):
62         if has_gripper:
63             if action == "close":
64                 gripper.close()
65             elif action == "open":
66                 gripper.open()
67             elif action == "calibrate":
68                 gripper.calibrate()
79     bindings = {
80         '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"),
81         'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"),
82         '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"),
83         'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"),
84         '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"),
85         'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"),
86         '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"),
87         'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"),
88         '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"),
89         't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"),
90         '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"),
91         'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"),
92         '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"),
93         'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease"),
94         '8': (set_g, "close", side+" gripper close"),
95         'i': (set_g, "open", side+" gripper open"),
96         '9': (set_g, "calibrate", side+" gripper calibrate")
97      }
89     done = False
90     print("Controlling joints. Press ? for help, Esc to quit.")
91     while not done and not rospy.is_shutdown():
92         c = intera_external_devices.getch()
93         if c:
94             #catch Esc or ctrl-c
95             if c in ['\x1b', '\x03']:
96                 done = True
97                 rospy.signal_shutdown("Example finished.")
 98             elif c in bindings:
 99                 cmd = bindings[c]
100                 if c == '8' or c == 'i' or c == '9':
101                     cmd[0](cmd[1])
102                     print("command: %s" % (cmd[2],))
103                 else:
104                     #expand binding to something like "set_j(right, 'j0', 0.1)"
105                     cmd[0](*cmd[1])
106                     print("command: %s" % (cmd[2],))
107             else:
108                 print("key bindings: ")
109                 print("  Esc: Quit")
110                 print("  ?: Help")
111                 for key, val in sorted(bindings.items(),
112                                        key=lambda x: x[1][2]):
113                     print("  %s: %s" % (key, val[2]))
115 def main():
116     """RSDK Joint Position Example: Keyboard Control
117     Use your dev machine's keyboard to control joint positions.
118     Each key corresponds to increasing or decreasing the angle
119     of a joint on Sawyer's arm. The increasing and descreasing
120     are represented by number key and letter key next to the number.
121     """
122     epilog = """
123 See help inside the example with the '?' key for key bindings.
124     """
125     rp = intera_interface.RobotParams()
126     valid_limbs = rp.get_limb_names()
127     if not valid_limbs:
128         rp.log_message(("Cannot detect any limb parameters on this robot. "
129                         "Exiting."), "ERROR")
130         return
131     arg_fmt = argparse.RawDescriptionHelpFormatter
132     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
133                                      description=main.__doc__,
134                                      epilog=epilog)
135     parser.add_argument(
136         "-l", "--limb", dest="limb", default=valid_limbs[0],
137         choices=valid_limbs,
138         help="Limb on which to run the joint position keyboard example"
139     )
140     args = parser.parse_args(rospy.myargv()[1:])
141 
142     print("Initializing node... ")
143     rospy.init_node("sdk_joint_position_keyboard")
144     print("Getting robot state... ")
145     rs = intera_interface.RobotEnable(CHECK_VERSION)
146     init_state = rs.state().enabled
150     def clean_shutdown():
151         print("\nExiting example.")
152         if not init_state:
153             print("Disabling robot...")
154             rs.disable()
155     rospy.on_shutdown(clean_shutdown)
157     rospy.loginfo("Enabling robot...")
158     rs.enable()
159     map_keyboard(args.limb)
160     print("Done.")
161 
162 
163 if __name__ == '__main__':
164     main()


Joint Position Joystick Code Walkthrough

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 
40 from intera_interface import CHECK_VERSION
43 def rotate(l):
44     """
45     Rotates a list left.
46     @param l: the list
47     """
48     if len(l):
49         v = l[0]
50         l[:-1] = l[1:]
51         l[-1] = v
54 def set_j(cmd, limb, joints, index, delta):
55     """
56     Set the selected joint to current pos + delta.
57     @param cmd: the joint command dictionary
58     @param limb: the limb to get the pos from
59     @param joints: a list of joint names
60     @param index: the index in the list of names
61     @param delta: delta to update the joint by
62     joint/index is to make this work in the bindings.
63     """
64     joint = joints[index]
65     cmd[joint] = delta + limb.joint_angle(joint)
69 def map_joystick(joystick, side):
70     """
71     Maps joystick input to joint position commands.
72     @param joystick: an instance of a Joystick
73     """
74     limb = intera_interface.Limb(side)
75     gripper = None
76     try:
77         gripper = intera_interface.Gripper(side)
78     except:
79         rospy.loginfo("Could not detect a connected electric gripper.")
82     def set_g(action):
83         if gripper is not None:
84             if action == "close":
85                 gripper.close()
86             elif action == "open":
87                 gripper.open()
88             elif action == "calibrate":
89                 gripper.calibrate()
 91     limb_cmd = {}
 92 
 93     #available joints
 94     joints = limb.joint_names()
 95 
 96     #abbreviations
 97     jhi = lambda s: joystick.stick_value(s) > 0
 98     jlo = lambda s: joystick.stick_value(s) < 0
 99     bdn = joystick.button_down
100     bup = joystick.button_up
102     def print_help(bindings_list):
103         print("Press Ctrl-C to quit.")
104         for bindings in bindings_list:
105             for (test, _cmd, doc) in bindings:
106                 if callable(doc):
107                     doc = doc()
108                 print("%s: %s" % (str(test[1][0]), doc))
109 
110     bindings_list = []
111     bindings = [
112         ((jlo, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, 0.1]),
113             lambda: "Increase " + joints[0]),
114         ((jhi, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, -0.1]),
115             lambda: "Decrease " + joints[0]),
116         ((jlo, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, 0.1]),
117             lambda: "Increase " + joints[1]),
118         ((jhi, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, -0.1]),
119             lambda: "Decrease " + joints[1]),
120         ((jlo, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, 0.1]),
121             lambda: "Increase " + joints[2]),
122         ((jhi, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, -0.1]),
123             lambda: "Decrease " + joints[2]),
124         ((jlo, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, 0.1]),
125             lambda: "Increase " + joints[3]),
126         ((jhi, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, -0.1]),
127             lambda: "Decrease " + joints[3]),
128         ((bdn, ['leftBumper']), (rotate, [joints]), side + ": cycle joint"),
129         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
130         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
131         ]
132     if gripper:
133         bindings.extend([
134             ((bdn, ['rightTrigger']), (set_g, ['close'], gripper), side + " gripper close"),
135             ((bup, ['rightTrigger']), (set_g, ['open'], gripper), side + " gripper open"),
136             ((bdn, ['btnLeft']), (set_g, ['calibrate'], gripper), "right calibrate")
137             ])
138     bindings_list.append(bindings)
139 
140     rate = rospy.Rate(100)
141     print_help(bindings_list)
142     print("Press Ctrl-C to stop. ")
143     while not rospy.is_shutdown():
144         for (test, cmd, doc) in bindings:
145             if test[0](*test[1]):
146                 cmd[0](*cmd[1])
147                 if callable(doc):
148                     print(doc())
149                 else:
150                     print(doc)
151         if len(limb_cmd):
152             limb.set_joint_positions(limb_cmd)
153             limb_cmd.clear()
154         rate.sleep()
155     return False
158 def main():
159     """SDK Joint Position Example: Joystick Control
160     Use a game controller to control the angular joint positions
161     of Sawyer's arms.
162     Attach a game controller to your dev machine and run this
163     example along with the ROS joy_node to control the position
164     of each joint in Sawyer's arm using the joystick. Be sure to
165     provide your *joystick* type to setup appropriate key mappings.
166     Each stick axis maps to a joint angle; which joints are currently
167     controlled can be incremented by using the mapped increment buttons.
168     Ex:
169       (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
170     """
171     epilog = """
172 See help inside the example with the "Start" button for controller
173 key bindings.
174     """
175     rp = intera_interface.RobotParams()
176     valid_limbs = rp.get_limb_names()
177     if not valid_limbs:
178         rp.log_message(("Cannot detect any limb parameters on this robot. "
179                         "Exiting."), "ERROR")
180         return
181     arg_fmt = argparse.RawDescriptionHelpFormatter
182     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
183                                      description=main.__doc__,
184                                      epilog=epilog)
185     required = parser.add_argument_group('required arguments')
186     required.add_argument(
187         '-j', '--joystick', required=True,
188         choices=['xbox', 'logitech', 'ps3'],
189         help='specify the type of joystick to use'
190     )
191     parser.add_argument(
192         "-l", "--limb", dest="limb", default=valid_limbs[0],
193         choices=valid_limbs,
194         help="Limb on which to run the joint position joystick example"
195     )
196     args = parser.parse_args(rospy.myargv()[1:])
197 
198     joystick = None
199     if args.joystick == 'xbox':
200         joystick = intera_external_devices.joystick.XboxController()
201     elif args.joystick == 'logitech':
202         joystick = intera_external_devices.joystick.LogitechController()
203     elif args.joystick == 'ps3':
204         joystick = intera_external_devices.joystick.PS3Controller()
205     else:
206         parser.error("Unsupported joystick type '%s'" % (args.joystick))
207 
208     print("Initializing node... ")
209     rospy.init_node("sdk_joint_position_joystick")
210     print("Getting robot state... ")
211     rs = intera_interface.RobotEnable(CHECK_VERSION)
212     init_state = rs.state().enabled
217     def clean_shutdown():
218         print("\nExiting example.")
219         if not init_state:
220             print("Disabling robot...")
221             rs.disable()
222     rospy.on_shutdown(clean_shutdown)
223 
224     rospy.loginfo("Enabling robot...")
225     rs.enable()
226 
227     map_joystick(joystick, args.limb)
228     print("Done.")
229 
230 
231 if __name__ == '__main__':
232     main()