Difference between revisions of "Joint Position Example"

Jump to: navigation , search
(Edit Joint Postition Ex)
(Code Walkthrough 2)
Line 94: Line 94:
 
<div class="content-block">
 
<div class="content-block">
  
== Code Walkthrough ==  
+
== Joint Position Keyboard Code Walkthrough ==  
  
 
Now, let's break down the code.
 
Now, let's break down the code.
  
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
"""
 +
SDK Joint Position Example: keyboard
 +
"""
 
import argparse
 
import argparse
import sys
 
  
 
import rospy
 
import rospy
  
 
import intera_interface
 
import intera_interface
 +
import intera_external_devices
 +
 +
from intera_interface import CHECK_VERSION
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the limb and the gripper class.
+
<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
  
<syntaxhighlight lang="python" line start="41" enclose="div">
+
     joints = limb.joint_names()
class Waypoints(object):
+
</syntaxhighlight>
     def __init__(self, speed, accuracy, limb="right"):
 
        # Create intera_interface limb instance
 
        self._arm = limb
 
        self._limb = intera_interface.Limb(self._arm)
 
  
         # Parameters which will describe joint position moves
+
<syntaxhighlight lang="python" line start="56" enclose="div">
         self._speed = speed
+
    def set_j(limb, joint_name, delta):
         self._accuracy = accuracy
+
         current_position = limb.joint_angle(joint_name)
 +
         joint_command = {joint_name: current_position + delta}
 +
         limb.set_joint_positions(joint_command)
  
         # Recorded waypoints
+
    def set_g(action):
        self._waypoints = list()
+
         if has_gripper:
 +
            if action == "close":
 +
                gripper.close()
 +
            elif action == "open":
 +
                gripper.open()
 +
            elif action == "calibrate":
 +
                gripper.calibrate()
 +
</syntaxhighlight>
  
         # Recording state
+
<syntaxhighlight lang="python" line start="79" enclose="div">
         self._is_recording = False
+
    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>
  
        # Verify robot is enabled
+
<syntaxhighlight lang="python" line start="89" enclose="div">
        print("Getting robot state... ")
+
    done = False
        self._rs = intera_interface.RobotEnable()
+
    print("Controlling joints. Press ? for help, Esc to quit.")
         self._init_state = self._rs.state().enabled
+
    while not done and not rospy.is_shutdown():
         print("Enabling robot... ")
+
         c = intera_external_devices.getch()
        self._rs.enable()
+
         if c:
 +
            #catch Esc or ctrl-c
 +
            if c in ['\x1b', '\x03']:
 +
                done = True
 +
                rospy.signal_shutdown("Example finished.")
 +
</syntaxhighlight>
  
        # Create Navigator I/O
+
<syntaxhighlight lang="python" line start="98" enclose="div">
        self._navigator_io = intera_interface.Navigator(self._arm)
+
            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>
  
An instance of the limb interface for the side under interest is created. An instance of the interface for the corresponding Navigator is also created. The robot is then enabled. It is important to note that the robot should be enabled in order to control the limb's movement.
+
<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:])
  
<syntaxhighlight lang="python" line start="67" enclose="div">
+
    print("Initializing node... ")
     def _record_waypoint(self, value):
+
    rospy.init_node("sdk_joint_position_keyboard")
        """
+
    print("Getting robot state... ")
        Stores joint position waypoints
+
     rs = intera_interface.RobotEnable(CHECK_VERSION)
 +
    init_state = rs.state().enabled
 +
</syntaxhighlight>
  
        Navigator 'OK/Wheel' button callback
+
<syntaxhighlight lang="python" line start="150" enclose="div">
         """
+
    def clean_shutdown():
         if value != 'OFF':
+
         print("\nExiting example.")
             print("Waypoint Recorded")
+
         if not init_state:
             self._waypoints.append(self._limb.joint_angles())
+
             print("Disabling robot...")
 +
             rs.disable()
 +
    rospy.on_shutdown(clean_shutdown)
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>joint_angles()</code> method retrieves the current joint angles and they are then appended to the <code>_waypoints</code> list.
+
<syntaxhighlight lang="python" line start="157" enclose="div">
 +
    rospy.loginfo("Enabling robot...")
 +
    rs.enable()
 +
    map_keyboard(args.limb)
 +
    print("Done.")
  
<syntaxhighlight lang="python" line start="77" enclose="div">
 
    def _stop_recording(self, value):
 
        """
 
        Sets is_recording to false
 
  
        Navigator 'Rethink' button callback
+
if __name__ == '__main__':
        """
+
    main()
        # On navigator Rethink button press, stop recording
 
        if value != 'OFF':
 
            print("Recording Stopped")
 
            self._is_recording = False
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This Rethink button's press event is captured.
 
  
<syntaxhighlight lang="python" line start="88" enclose="div">
+
</div>
    def record(self):
 
        """
 
        Records joint position waypoints upon each Navigator 'OK/Wheel' button
 
        press.
 
        """
 
        rospy.loginfo("Waypoint Recording Started")
 
        print("Press Navigator 'OK/Wheel' button to record a new joint "
 
        "joint position waypoint.")
 
        print("Press Navigator 'Rethink' button when finished recording "
 
              "waypoints to begin playback")
 
        # Connect Navigator callbacks
 
        # Navigator scroll wheel button press
 
        ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok')
 
        # Navigator Rethink button press
 
        show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show')
 
  
        # Set recording flag
+
== Joint Position Joystick Code Walkthrough ==
        self._is_recording = True
+
 
 +
Now, let's break down the code.
  
        # Loop until waypoints are done being recorded ('Rethink' Button Press)
+
<syntaxhighlight lang="python" line start="33" enclose="div">
        while not rospy.is_shutdown() and self._is_recording:
+
import argparse
            rospy.sleep(1.0)
 
  
        # We are now done with the navigator callbacks, disconnecting them
+
import rospy
        self._navigator.deregister_callback(ok_id)
+
 
        self._navigator.deregister_callback(show_id)
+
import intera_interface
 +
import intera_external_devices
 +
 
 +
from intera_interface import CHECK_VERSION
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The navigator 'OK/Wheel' button invokes the <code>_record_waypoint()</code> method on a press event, which records the current joint positions as explained above. Similarly, the navigator 'Rethink' button invokes the <code>_stop_recording</code> method on a press event, which sets the <code>is_recording</code> variable to false.
+
<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="115" enclose="div">
+
<syntaxhighlight lang="python" line start="54" enclose="div">
     def playback(self):
+
def set_j(cmd, limb, joints, index, delta):
        """
+
    """
        Loops playback of recorded joint position waypoints until program is
+
    Set the selected joint to current pos + delta.
        exited
+
    @param cmd: the joint command dictionary
        """
+
    @param limb: the limb to get the pos from
         rospy.sleep(1.0)
+
     @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>
  
         rospy.loginfo("Waypoint Playback Started")
+
<syntaxhighlight lang="python" line start="82" enclose="div">
        print(" Press Ctrl-C to stop...")
+
    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>
  
        # Set joint position speed ratio for execution
+
<syntaxhighlight lang="python" line start="91" enclose="div">
        self._limb.set_joint_position_speed(self._speed)
+
    limb_cmd = {}
  
        # Loop until program is exited
+
    #available joints
        loop = 0
+
    joints = limb.joint_names()
        while not rospy.is_shutdown():
 
            loop += 1
 
            print("Waypoint playback loop #%d " % (loop,))
 
            for waypoint in self._waypoints:
 
                if rospy.is_shutdown():
 
                    break
 
                self._limb.move_to_joint_positions(waypoint, timeout=20.0,
 
                                                  threshold=self._accuracy)
 
            # Sleep for a few seconds between playback loops
 
            rospy.sleep(3.0)
 
  
        # Set joint position speed back to default
+
    #abbreviations
        self._limb.set_joint_position_speed(0.3)
+
    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>
 
</syntaxhighlight>
  
The <code>set_joint_position_speed()</code> method sets the ratio of max joint speed to use during joint position moves. The method <code>move_to_joint_positions()</code>, moves the joints to the commanded position. It is important to note that there is not trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and goal positions are obtained. They are then published as a joint command message using the <code>set_joint_positions()</code> method. Thus, all the waypoints that were stored are visited along a smooth trajectory.
+
<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))
 +
 
 +
    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)
  
<syntaxhighlight lang="python" line start="144" enclose="div">
+
    rate = rospy.Rate(100)
     def clean_shutdown(self):
+
     print_help(bindings_list)
        print("\nExiting example...")
+
    print("Press Ctrl-C to stop. ")
        if not self._init_state:
+
    while not rospy.is_shutdown():
             print("Disabling robot...")
+
        for (test, cmd, doc) in bindings:
             self._rs.disable()
+
             if test[0](*test[1]):
        return True
+
                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>
  
On shutdown, the robot is sent to its initial state that was captured.
+
<syntaxhighlight lang="python" line start="158" enclose="div">
 
 
<syntaxhighlight lang="python" line start="152" enclose="div">
 
 
def main():
 
def main():
     """RSDK Joint Position Waypoints Example
+
     """SDK Joint Position Example: Joystick Control
 
+
     Use a game controller to control the angular joint positions
     Records joint positions each time the navigator 'OK/wheel'
+
    of Sawyer's arms.
     button is pressed.
+
    Attach a game controller to your dev machine and run this
     Upon pressing the navigator 'Rethink' button, the recorded joint positions
+
    example along with the ROS joy_node to control the position
    will begin playing back in a loop.
+
    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
 
     arg_fmt = argparse.RawDescriptionHelpFormatter
 
     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
 
     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
+
                                     description=main.__doc__,
     parser.add_argument(
+
                                    epilog=epilog)
         '-s', '--speed', default=0.3, type=float,
+
     required = parser.add_argument_group('required arguments')
         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
+
    required.add_argument(
 +
         '-j', '--joystick', required=True,
 +
        choices=['xbox', 'logitech', 'ps3'],
 +
         help='specify the type of joystick to use'
 
     )
 
     )
 
     parser.add_argument(
 
     parser.add_argument(
         '-a', '--accuracy',
+
         "-l", "--limb", dest="limb", default=valid_limbs[0],
        default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
+
        choices=valid_limbs,
         help='joint position accuracy (rad) at which waypoints must achieve'
+
         help="Limb on which to run the joint position joystick example"
 
     )
 
     )
 
     args = parser.parse_args(rospy.myargv()[1:])
 
     args = parser.parse_args(rospy.myargv()[1:])
</syntaxhighlight>
 
  
The speed and accuracy on which the example is to be demonstrated is captured from the command line arguments.
+
    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))
  
<syntaxhighlight lang="python" line start="178" enclose="div">
 
 
     print("Initializing node... ")
 
     print("Initializing node... ")
     rospy.init_node("rsdk_joint_position_waypoints")
+
     rospy.init_node("sdk_joint_position_joystick")
 +
    print("Getting robot state... ")
 +
    rs = intera_interface.RobotEnable(CHECK_VERSION)
 +
    init_state = rs.state().enabled
 +
</syntaxhighlight>
 +
 
 +
<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)
  
     waypoints = Waypoints(args.speed, args.accuracy)
+
     rospy.loginfo("Enabling robot...")
 +
    rs.enable()
  
     # Register clean shutdown
+
     map_joystick(joystick, args.limb)
     rospy.on_shutdown(waypoints.clean_shutdown)
+
     print("Done.")
  
    # Begin example program
 
    waypoints.record()
 
    waypoints.playback()
 
  
 
if __name__ == '__main__':
 
if __name__ == '__main__':
Line 286: Line 471:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The node is initialized and an instance of the <code>Waypoints</code> class is created. The user defined waypoints are recorded using the <code>record()</code> method and are played back using the <code>playback()</code> method as explained above.
 
  
 
</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()