Difference between pages "SDK Shell" and "Joint Position Example"

(Difference between pages)
Jump to: navigation , search
(Verify RSDK Shell)
 
(Joint Position Keyboard Code Walkthrough mini fix)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''This tutorial describes the configuration of the ROS environment on your developer workstation'''</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 ==
  
== Overview ==
+
Start the joint position keyboard example program, ex:
  
To communicate with and command your robot, we must establish the connection between your development PC and the robot. Assuming proper [[Networking | network setup]], the SDK shell refers to the a configuration of your [http://wiki.ros.org/ROS/EnvironmentVariables ROS environment] which points your PC to the [http://wiki.ros.org/Master ROS Master], while registering your IP or Hostname allowing other processes to find you.
+
<syntaxhighlight lang="bash" enclose="div">
 +
$ 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:
  
The SDK provides a convenient script, <code> intera.sh </code>, which helps foster the quickest and easiest ROS environment setup to get communicating with Sawyer. This script should have been installed/configured during [[Workstation_Setup | Workstation Setup Tutorial]], and the [[Hello Robot | Hello Robot! Tutorial]].
+
<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 19: Line 35:
 
<div class="content-block">
 
<div class="content-block">
  
== Quick Environment Setup Via intera.sh ==
+
== Arguments ==
 +
 
 +
'''Important Arguments:''' 
 +
 
 +
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">
 +
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
 +
</syntaxhighlight>
 +
 
 +
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">
$ cd ~/ros_ws
+
usage: joint_position_joystick.py [-h] [-l LIMB] [-j JOYSTICK]
$ ./intera.sh
+
 
 +
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
 
</syntaxhighlight>
 
</syntaxhighlight>
 +
  
 
</div>
 
</div>
Line 30: Line 94:
 
<div class="content-block">
 
<div class="content-block">
  
== Detailed Description/Setup ==
+
== Joint Position Keyboard Code Walkthrough ==  
 +
 
 +
Now, let's break down the code.
  
=== intera.sh ROS Environment Setup ===
+
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
"""
 +
SDK Joint Position Example: keyboard
 +
"""
 +
import argparse
  
The intera.sh script is a convenient script allowing users to get their ROS environment setup quickly. It should allow for intuitive modification of the core components to get communicating with Sawyer setup quickly. It creates a heredoc which verifies all environment variables expected are provided, with a handy sim argument for local configurations (useful for simulation), and local argument (useful when SSH'd into Sawyer with no need to communicate to external, off-robot processes). This subshell created has the custom prompt modifications (with color) for quick visual checks as well.
+
import rospy
  
=== Checkout Convenient intera.sh script ===
+
import intera_interface
 +
import intera_external_devices
  
Copy the intera.sh file from intera_sdk repo to ros workspace.
+
from intera_interface import CHECK_VERSION
 +
</syntaxhighlight>
  
=== Customize the intera.sh script ===
+
This imports the intera interface 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.
  
Please edit the intera.sh shell script making the necessary modifications to describe your development PC.
+
<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
  
Using your favorite editor ([https://wiki.gnome.org/Apps/Gedit gedit] used for example)
+
    joints = limb.joint_names()
<syntaxhighlight lang="bash" enclose="div">
 
$ cd ~/ros_ws
 
$ gedit intera.sh
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
=== Edit the 'robot_hostname' field ===
+
The instance of Limb class, <code>limb</code> and the instance of Gripper class, <code>gripper</code> are created. <code>joint_names()</code> method returns an array of joints of the limb.
 +
 
 +
<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)
  
Please edit the 'robot_hostname' field:
+
    def set_g(action):
<syntaxhighlight lang="bash" enclose="div">
+
        if has_gripper:
# Specify robot's hostname
+
            if action == "close":
**robot_hostname="robot_hostname"**
+
                gripper.close()
 +
            elif action == "open":
 +
                gripper.open()
 +
            elif action == "calibrate":
 +
                gripper.calibrate()
 
</syntaxhighlight>
 
</syntaxhighlight>
Modifying where 'robot_hostname' is the [Robot_Hostname hostname of your robot]
 
  
=== Edit 'your_ip' OR 'your_hostname' field ===
+
The <code>set_j()</code> function is invoked whenever a valid key press occurs. The <code>limb</code> refers to the limb instance of Sawyer's limb. <code>delta</code> refers to the required displacement of the joint from its current position. The method <code>joint_angle()</code> returns the current position of that joint. Then the joint command message is updated for that corresponding joint to indicate the new position. <code>set_joint_position()</code> method publishes the joint commands to the position controller.
 +
The <code>set_g</code> function calls gripper action function(<code>open()</code>, or <code>close()</code> or <code>calibrate()</code>) when corresponding action is provided.
  
Please edit the 'your_ip' field (only if not using 'your_hostname'):
+
<syntaxhighlight lang="python" line start="70" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
    bindings = {
**your_ip="192.168.XXX.XXX"**
+
        '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>
Modifying where 'your_ip' is the IP address of your PC.
 
  
Useful command for identifying your IP address:
+
The <code>bindings</code> is a dictionary that holds the set of characters in the keyboard and their corresponding joints.
<syntaxhighlight lang="bash" enclose="div">
+
 
$ ifconfig
+
<syntaxhighlight lang="python" line start="89" enclose="div">
# Result will be contained in the 'inet addr' field (Ubuntu) or 'inet' field (Gentoo Robot PC).
+
    done = False
 +
    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>
  
'''Important:''' Only set either ROS_IP '''*OR*''' ROS_HOSTNAME.
+
The <code>done</code> variable captures whether "esc" or "ctrl-c" was hit. The while loop iterates as long as the "esc" or "ctrl-c" is hit or ros-shutdown signal is given. c captures the keyboard input. If "esc" or "ctrl-c" is given then <code>done</code> gets updated as true, and a shutdown signal will be sent.
  
Setting of both ROS_IP and ROS_HOSTNAME will result in ROS_HOSTNAME taking priority, resulting in confusing environment setups. Comment out the unused method using the '#' as leading character.
+
<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>
  
Alternatively, you may choose to use the hostname of your development PC.  
+
The user inputted key is checked whether it is in the <code>bindings</code> dictionary. <code>bindings[c]</code> returns the value of the key c from the dictionary. The 0th index of the value refers to the function to be called and the next index holds the arguments to be sent along with that function. <code>cmd[2]</code> is the description of the joint command from the dictionary above.
  
Please edit the 'your_hostname' field (only if not using 'your_ip'):
+
<syntaxhighlight lang="python" line start="107" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
            else:
***your_hostname="my_computer.local***
+
                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>
Modifying where 'your_hostname' is the hostname of your PC.
 
  
'''Important:''' This hostname must be resolvable to the robot. To test if your hostname is resolvable to Sawyer, please visit the development PC [[Networking]] page.
+
This case is executed when the key pressed is not a valid input. So, the key is sorted and is printed along with its corresponding description.
 +
 
 +
<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>
  
=== Edit 'ros_version' field ===
+
The node is initialized and then the software version is checked for compatiblity with the local version. <code>init_state</code> captures Sawyer's initial state. This is to make sure that Sawyer is sent back to the same state after the program exits.
  
Please edit the 'ros_version' field:
+
<syntaxhighlight lang="python" line start="150" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
    def clean_shutdown():
***ros_version="indigo"***
+
        print("\nExiting example.")
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
 
</syntaxhighlight>
 
</syntaxhighlight>
Modifying where 'ros_version' is the your chosen ROS release.
 
  
=== Save and Close intera.sh script ===
+
As explained above, the robot is checked if it was initially disabled and if so, it is disabled upon the program's termination.
  
Please save and close the intera.sh script.
+
<syntaxhighlight lang="python" line start="157" enclose="div">
 +
    rospy.loginfo("Enabling robot...")
 +
    rs.enable()
 +
    map_keyboard(args.limb)
 +
    print("Done.")
  
=== Initialize your SDK environment ===
 
  
From this point forward, your ROS environment setup should be as simple as running the intera.sh script from the root of your Catkin workspace:
+
if __name__ == '__main__':
<syntaxhighlight lang="bash" enclose="div">
+
    main()
$ cd ~/ros_ws
 
$ ./intera.sh
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
You will see that your current shell prompt will be prefixed with:
+
The robot is enabled before the programs execution. It is important to note that Sawyer should be auto-enabled before trying to move its joints. <code>map_keyboard()</code> function captures the key input and moves the joint as explained above.
<syntaxhighlight lang="bash" enclose="div">
 
    [intera - http://<robot_hostname>:11311]username@machine$
 
</syntaxhighlight>
 
This allows you to quickly view if your shell is configured for intera communication or not, and to which robot you are currently addressing (<code><robot_hostname></code>)
 
  
 
</div>
 
</div>
Line 117: Line 279:
 
<div class="content-block">
 
<div class="content-block">
  
== Verify RSDK Shell ==
+
== Joint Position Joystick Code Walkthrough ==  
 +
 
 +
Now, let's break down the code.
 +
 
 +
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import argparse
 +
 
 +
import rospy
  
=== Test ability to ping robot. ===
+
import intera_interface
 +
import intera_external_devices
  
Ping what you have previously set as your <code>robot_hostname</code>.
+
from intera_interface import CHECK_VERSION
<syntaxhighlight lang="bash" enclose="div">
 
$ ping <robot_hostname>
 
# ex.
 
$ ping 011304P0026
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
If unable to ping the robot visit [[Connecting and Testing the Development Workstation|this page]].
+
<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>
  
=== Test ability to command the robot ===
+
<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>
  
1. Enable the robot
+
<syntaxhighlight lang="python" line start="69" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
def map_joystick(joystick, side):
$ rosrun intera_tools enable_robot.py -e
+
    """
 +
    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>
 
</syntaxhighlight>
The robot is now active. You may now grasp Sawyer's arm freely by grasping the cuff at the hand.
 
  
2. Disable the robot
+
<syntaxhighlight lang="python" line start="82" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
    def set_g(action):
$ rosrun intera_tools enable_robot.py -d
+
        if gripper is not None:
 +
            if action == "close":
 +
                gripper.close()
 +
            elif action == "open":
 +
                gripper.open()
 +
            elif action == "calibrate":
 +
                gripper.calibrate()
 
</syntaxhighlight>
 
</syntaxhighlight>
You should visibly see Sawyer's arm gently fall and will no longer have the ability to move the arm freely.
 
  
If unsuccessful in enabling the robot, please follow the instructions below. This is typically due to a ROS_HOSTNAME being set which is not resolvable to Sawyer. Please unset your ROS_HOSTNAME, instead using your ROS_IP. Make sure this is reflected permanently in your <code> intera.sh </code> script.
+
<syntaxhighlight lang="python" line start="91" enclose="div">
 +
    limb_cmd = {}
 +
 
 +
    #available joints
 +
    joints = limb.joint_names()
 +
 
 +
    #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>
  
=== Timeout observed when trying to enable the robot. ===
+
<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))
  
Assuming that you are able to <code>$rostopic echo /rosout</code> during [[Connecting and Testing the Development Workstation#steps-for-connecting-to-sawyer|these prerequisite instructions]]. This error is a sign of the inability to make '''commands''' to the robot. This is due to your ROS_HOSTNAME/ROS_IP being set incorrectly.
+
    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)
  
View the ROS_ENVIRONMENT set for your current shell.
+
    rate = rospy.Rate(100)
<syntaxhighlight lang="bash" enclose="div">
+
    print_help(bindings_list)
$ env | grep ROS
+
    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>
 
</syntaxhighlight>
Verify that '''***EITHER***''' the ROS_HOSTNAME or ROS_IP is set for your development workstation.
 
  
If unsuccessful and the ROS_HOSTNAME is set, please unset this variable and use ROS_IP.
+
<syntaxhighlight lang="python" line start="158" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
def main():
$ unset ROS_HOSTNAME
+
    """SDK Joint Position Example: Joystick Control
$ export ROS_IP=<your_development_workstation_ip>
+
    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:])
 +
 
 +
    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))
 +
 
 +
    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>
 
</syntaxhighlight>
  
Note: You can always view your current development workstation IP address using
+
<syntaxhighlight lang="python" line start="217" enclose="div">
<syntaxhighlight lang="bash" enclose="div">
+
    def clean_shutdown():
$ ifconfig
+
        print("\nExiting example.")
 +
        if not init_state:
 +
            print("Disabling robot...")
 +
            rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
 +
 
 +
    rospy.loginfo("Enabling robot...")
 +
    rs.enable()
 +
 
 +
    map_joystick(joystick, args.limb)
 +
    print("Done.")
 +
 
 +
 
 +
if __name__ == '__main__':
 +
    main()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
If unsuccessful and the ROS_IP is set, verify that this is describes '''your''' ip address and not the '''robot''' ip address.
+
Put comments TODO
<syntaxhighlight lang="bash" enclose="div">
 
# Verify ROS_HOSTNAME is unset
 
$ unset ROS_HOSTNAME
 
# Take note of your IP address
 
$ ifconfig
 
# Export this as your ROS_IP
 
$ export ROS_IP=<your_development_workstation_ip>
 
</syntaxhighlight>
 
  
 
</div>
 
</div>

Revision as of 09:23, 28 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

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.

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()

The instance of Limb class, limb and the instance of Gripper class, gripper are created. joint_names() method returns an array of joints of the limb.

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()

The set_j() function is invoked whenever a valid key press occurs. The limb refers to the limb instance of Sawyer's limb. delta refers to the required displacement of the joint from its current position. The method joint_angle() returns the current position of that joint. Then the joint command message is updated for that corresponding joint to indicate the new position. set_joint_position() method publishes the joint commands to the position controller. The set_g function calls gripper action function(open(), or close() or calibrate()) when corresponding action is provided.

70     bindings = {
71         '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"),
72         'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"),
73         '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"),
74         'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"),
75         '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"),
76         'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"),
77         '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"),
78         'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"),
79         '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"),
80         't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"),
81         '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"),
82         'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"),
83         '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"),
84         'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease"),
85         '8': (set_g, "close", side+" gripper close"),
86         'i': (set_g, "open", side+" gripper open"),
87         '9': (set_g, "calibrate", side+" gripper calibrate")
88      }

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

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.")

The done variable captures whether "esc" or "ctrl-c" was hit. The while loop iterates as long as the "esc" or "ctrl-c" is hit or ros-shutdown signal is given. c captures the keyboard input. If "esc" or "ctrl-c" is given then done gets updated as true, and a shutdown signal will be sent.

 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],))

The user inputted key is checked whether it is in the bindings dictionary. bindings[c] returns the value of the key c from the dictionary. The 0th index of the value refers to the function to be called and the next index holds the arguments to be sent along with that function. cmd[2] is the description of the joint command from the dictionary above.

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]))

This case is executed when the key pressed is not a valid input. So, the key is sorted and is printed along with its corresponding description.

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

The node is initialized and then the software version is checked for compatiblity with the local version. init_state captures Sawyer's initial state. This is to make sure that Sawyer is sent back to the same state after the program exits.

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)

As explained above, the robot is checked if it was initially disabled and if so, it is disabled upon the program's termination.

157     rospy.loginfo("Enabling robot...")
158     rs.enable()
159     map_keyboard(args.limb)
160     print("Done.")
161 
162 
163 if __name__ == '__main__':
164     main()

The robot is enabled before the programs execution. It is important to note that Sawyer should be auto-enabled before trying to move its joints. map_keyboard() function captures the key input and moves the joint as explained above.

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()

Put comments TODO