def main():
    """SDK Navigator Example

    Demonstrates Navigator by echoing input values from wheels and
    buttons.

    Uses the intera_interface.Navigator class to demonstrate an
    example of using the register_callback feature.
        
     Shows Navigator input of the arm for 10 seconds.
    """
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-n", "--navigator", dest="nav_name", default="right",
        choices=["right", "head"],
        help='Navigator on which to run example'
        )
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('sdk_navigator', anonymous=True)
    echo_input(args.nav_name)
    return 0 

def main():
    """RSDK Head Example: Wobbler

    Nods the head and pans side-to-side towards random angles.
    Demonstrates the use of the intera_interface.Head class.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_head_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    print("Wobbling... ")
    wobbler.wobble()
    print("Done.") 

def main():
    """Intera RSDK Joint Velocity Example: Wobbler

    Commands joint velocities of randomly parameterized cosine waves
    to each joint. Demonstrates Joint Velocity Control Mode.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_velocity_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    wobbler.wobble()

    print("Done.") 

def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )

    args = parser.parse_args(rospy.myargv()[1:])
    limb = args.limb

    rospy.init_node("gripper")

    gripper = GripMover(limb)

    #Wait on signal from visual_servo
    while not rospy.is_shutdown() and not gripper.done:
        pass 

def main():
    """RSDK Gripper Example: send a command to control the grippers.

    Run this example to command various gripper movements while
    adjusting gripper parameters, including calibration, and velocity:
    Uses the intera_interface.Gripper class and the
    helper function, intera_external_devices.getch.
    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    rp = intera_interface.RobotParams()
    valid_limbs = rp.get_limb_names()
    if not valid_limbs:
        rp.log_message(("Cannot detect any limb parameters on this robot. "
                        "Exiting."), "ERROR")
        return
    limb = valid_limbs[0]
    print("Using limb: {}.".format(limb))
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.add_argument(
        "-a", "--action", dest="action", default="open",
        choices=["open", "close"],
        help="Action to perform with the gripper. Options: close or open"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_gripper_keyboard")

    move_gripper(limb, args.action) 

def main():
    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
    # Add an option for starting a server for all valid limbs
    all_limbs = valid_limbs
    all_limbs.append("all_limbs")
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=all_limbs,
        help="joint trajectory action server limb"
    )
    parser.add_argument(
        "-r", "--rate", dest="rate", default=100.0,
        type=float, help="trajectory control rate (Hz)"
    )
    parser.add_argument(
        "-m", "--mode", default='position_w_id',
        choices=['position_w_id', 'position', 'velocity'],
        help="control mode for trajectory execution"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    start_server(args.limb, args.rate, args.mode, valid_limbs) 

def main():
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument("-g", "--gripper", dest="gripper", default="both",
                        choices=['both', 'left', 'right'],
                        help="gripper action server limb", )
    args = parser.parse_args(rospy.myargv()[1:])
    start_server(args.gripper) 

def main():
    """RSDK URDF Fragment Example:
    This example shows a proof of concept for
    adding your URDF fragment to the robot's
    onboard URDF (which is currently in use).
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='Path to URDF file to send'
    )
    required.add_argument(
        '-l', '--link', required=False, default="right_hand", #parent
        help='URDF Link already to attach fragment to (usually <left/right>_hand)'
    )
    required.add_argument(
        '-j', '--joint', required=False, default="right_gripper_base",
        help='Root joint for fragment (usually <left/right>_gripper_base)'
    )
    parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)),
            default=5.0, help="[in seconds] Duration to publish fragment")
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('rsdk_configure_urdf', anonymous=True)

    if not os.access(args.file, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (args.file,))
        return 1
    send_urdf(args.link, args.joint, args.file, args.duration)
    return 0 

def main():
    """Save / Load EndEffector Config utility

    Read current config off of ClickSmart and save to file.
    Or load config from file and reconfigure and save it to ClickSmart device.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)

    parser.add_argument(
        '-s', '--save', metavar='PATH',
        help='save current EE config to given file'
    )
    parser.add_argument(
        '-l', '--load', metavar='PATH',
        help='load config from given file onto EE'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node('ee_config_editor', anonymous=True)

    ee = intera_interface.get_current_gripper_interface()
    if not ee:
        rospy.logerr("Could not detect an attached EndEffector!")
        return

    if args.save:
        rospy.loginfo("Saving EE config to {}".format(args.save))
        save_config(ee, args.save)

    if args.load:
        rospy.loginfo("Loading config and writing config to ClickSmart from {}".format(args.load))
        load_config(ee, args.load)

    def clean_shutdown():
        print("\nExiting example...")

    rospy.on_shutdown(clean_shutdown) 

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('-l', '--limb',
                        choices=['left', 'right'], default="right",
                        help="Calibrate the specified arm")
    args = parser.parse_args(rospy.myargv()[1:])
    arm = args.limb

    print("Initializing node...")
    rospy.init_node('sdk_calibrate_arm_{0}'.format(arm), disable_signals=True)

    rospy.loginfo("Preparing to Calibrate...")
    gripper_warn = ("IMPORTANT: Make sure to remove grippers and other"
                    " attachments before running Calibrate.")
    rospy.loginfo(gripper_warn)
    if not is_gripper_removed():
        return 1

    ca = CalibrateArm(arm)

    error = None
    goal_state = "unreported error"
    rospy.loginfo("Running Calibrate on {0} arm".format(arm))
    try:
        goal_state = ca.start_calibration()
    except KeyboardInterrupt, e:
        error = e
        goal_state = ca.stop_calibration() 

def main():
    """RSDK URDF Fragment Example:
    This example shows a proof of concept for
    adding your URDF fragment to the robot's
    onboard URDF (which is currently in use).
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='Path to URDF file to send'
    )
    required.add_argument(
        '-l', '--link', required=False, default="right_hand",
        help='URDF Link already to attach fragment to (usually <left/right>_hand)'
    )
    required.add_argument(
        '-j', '--joint', required=False, default="right_gripper_base",
        help='Root joint for fragment (usually <left/right>_gripper_base)'
    )
    parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)),
            default=5.0, help="[in seconds] Duration to publish fragment")
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('rsdk_configure_urdf', anonymous=True)

    if not os.access(args.file, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (args.file,))
        return 1
    send_urdf(args.link, args.joint, args.file, args.duration)
    return 0 

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('-s', '--state', const='state',
                        dest='actions', action='append_const',
                        help='Print current robot state')
    parser.add_argument('-e', '--enable', const='enable',
                        dest='actions', action='append_const',
                        help='Enable the robot')
    parser.add_argument('-d', '--disable', const='disable',
                        dest='actions', action='append_const',
                        help='Disable the robot')
    parser.add_argument('-r', '--reset', const='reset',
                        dest='actions', action='append_const',
                        help='Reset the robot')
    parser.add_argument('-S', '--stop', const='stop',
                        dest='actions', action='append_const',
                        help='Stop the robot')
    args = parser.parse_args(rospy.myargv()[1:])

    if args.actions == None:
        parser.print_usage()
        parser.exit(0, "No action defined")

    rospy.init_node('sdk_robot_enable')
    rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)

    try:
        for act in args.actions:
            if act == 'state':
                print rs.state()
            elif act == 'enable':
                rs.enable()
            elif act == 'disable':
                rs.disable()
            elif act == 'reset':
                rs.reset()
            elif act == 'stop':
                rs.stop()
    except Exception, e:
        rospy.logerr(e.strerror) 

def main():
    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
    # Add an option for starting a server for all valid limbs
    all_limbs = valid_limbs
    all_limbs.append("all_limbs")
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=all_limbs,
        help="joint trajectory action server limb"
    )
    parser.add_argument(
        "-r", "--rate", dest="rate", default=100.0,
        type=float, help="trajectory control rate (Hz)"
    )
    parser.add_argument(
        "-m", "--mode", default='position_w_id',
        choices=['position_w_id', 'position', 'velocity'],
        help="control mode for trajectory execution"
    )
    args = parser.parse_args(rospy.myargv()[1:])
    start_server(args.limb, args.rate, args.mode, valid_limbs) 

def main():
    """RSDK Gripper Example: Keyboard Control

    Use your dev machine's keyboard to control and configure grippers.

    Run this example to command various gripper movements while
    adjusting gripper parameters, including calibration, and velocity:
    Uses the intera_interface.Gripper class and the
    helper function, intera_external_devices.getch.
    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    rp = intera_interface.RobotParams()
    valid_limbs = rp.get_limb_names()
    if not valid_limbs:
        rp.log_message(("Cannot detect any limb parameters on this robot. "
                        "Exiting."), "ERROR")
        return
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=valid_limbs,
        help="Limb on which to run the gripper keyboard example"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_gripper_keyboard")

    map_keyboard(args.limb) 

def main():
    """SDK Joint Position Waypoints Example

    Records joint positions each time the navigator 'OK/wheel'
    button is pressed.
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
    will begin playing back in a loop.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.add_argument(
        '-s', '--speed', default=0.3, type=float,
        help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
    )
    parser.add_argument(
        '-a', '--accuracy',
        default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
        help='joint position accuracy (rad) at which waypoints must achieve'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_joint_position_waypoints", anonymous=True)

    waypoints = Waypoints(args.speed, args.accuracy)

    # Register clean shutdown
    rospy.on_shutdown(waypoints.clean_shutdown)

    # Begin example program
    waypoints.record()
    waypoints.playback() 

def main():
    """Intera SDK Lights Example: Blink

    Toggles the Lights interface on then off again
    while printing the state at each step. Simple demonstration
    of using the intera_interface.Lights class.

    Run this example with default arguments and watch the green
    light on the head blink on and off while the console
    echos the state. Use the light names from Lights.list_all_lights()
    to change lights to toggle.
    """
    epilog = """ Intera Interface Lights """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.add_argument(
        '-l', '--light_name', dest='light_name',
        default='head_green_light',
        help=('name of Light component to use'
              ' (default: head_green_light)')
    )
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('sdk_lights_blink', anonymous=True)
    test_light_interface(args.light_name) 

def start_proxies():  
    parser = argparse.ArgumentParser()
    
    parser.add_argument('url', help='The url address of robot cloud.')
    
    parser.add_argument('--services', '--srv', nargs='+', help='Services provided by ROSBridge server')
    parser.add_argument('--published_topics', '--pub', nargs='+', help='Topics published to ROSBridge server')
    parser.add_argument('--subscribed_topics', '--sub', nargs='+', help='Topics subscribed from ROSBrdge server')
    parser.add_argument('--actions', nargs='+', help='Actions provided by ROSBridge server')
    parser.add_argument('-q', '--queue_size', type=int, default=1000, help='ROS message queue size on each topic')
    parser.add_argument('-t', '--test', action='store_true', default=False, help='Use if server and client are using the same ROS master for testing. Client service and topic names will have _ws appended.')
    parser.add_argument('-i', '--image_id', help='Unique image id on the robot cloud.', default="")
    
    args = parser.parse_args(rospy.myargv()[1:])
     
    if not args.url.startswith('http'):
        args.url = 'http://' + args.url
    httpurl = args.url + '/getinstance/' + args.image_id
    try:
        req = urllib2.Request(httpurl)
        url_and_containerid = urllib2.urlopen(req).read()
	wsurl = url_and_containerid.split(" ")[0]
	containerid = url_and_containerid.split(" ")[1]
    except Exception, e:
        rospy.logerr('Failed to get websocket address for image %s from %s. Reason: %s', args.image_id, httpurl, str(e))
        return 

def main():

    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=False, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )

    args = parser.parse_args(rospy.myargv()[1:])
    if args.limb is None:
        limb = 'right'
    else:
        limb = args.limb

    rospy.init_node("get_goal_poses")

    pose_calc = PoseCalculator(limb)
    pose_calc.subscribe()
    pose_calc.publish()
    rate = rospy.Rate(params['rate'])
    while not rospy.is_shutdown():
        if len(pose_calc.goal_poses) > 0:
            print pose_calc.goal_poses
            pose_msg = PoseArray()
            pose_msg.poses = pose_calc.goal_poses
            pose_calc.handler_pub.publish(pose_msg)
        rate.sleep() 

def main():
    """RSDK End Effector Position Example: Keyboard Control

    Use your dev machine's keyboard to control end effector positions.

    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_position_keyboard")
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example...")
        if not init_state:
            print("Disabling robot...")
            rs.disable()
    rospy.on_shutdown(clean_shutdown)

    print("Enabling robot... ")
    rs.enable()

    map_keyboard()
    print("Done.") 

def __init__(self, name):
        """
        :param name: the name of the ROS node
        """
        super(NaoqiNode, self).__init__()

        # A distutils.version.LooseVersion that contains the current verssion of NAOqi we're connected to
        self.__naoqi_version = None
        self.__name = name

        ## NAOqi stuff
        # dict from a modulename to a proxy
        self.__proxies = {}

        # Initialize ros, before getting parameters.
        rospy.init_node(self.__name)

        # If user has set parameters for ip and port use them as default
        default_ip = rospy.get_param("~pip", "127.0.0.1")
        default_port = rospy.get_param("~pport", 9559)

        # get connection from command line:
        from argparse import ArgumentParser
        parser = ArgumentParser()
        parser.add_argument("--pip", dest="pip", default=default_ip,
                          help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP")
        parser.add_argument("--pport", dest="pport", default=default_port, type=int,
                          help="port of parent broker. Default is 9559.", metavar="PORT")

        import sys
        args, unknown = parser.parse_known_args(args=rospy.myargv(argv=sys.argv)[1:])
        self.pip = args.pip
        self.pport = args.pport

        ## ROS stuff
        self.__stop_thread = False
        # make sure that we unregister from everything when the module dies
        rospy.on_shutdown(self.__on_ros_shutdown) 

def main():
    """RSDK Joint Torque Example: Joint Springs

    Moves the default limb to a neutral location and enters
    torque control mode, attaching virtual springs (Hooke's Law)
    to each joint maintaining the start position.

    Run this example and interact by grabbing, pushing, and rotating
    each joint to feel the torques applied that represent the
    virtual springs attached. You can adjust the spring
    constant and damping coefficient for each joint using
    dynamic_reconfigure.
    """
    # Querying the parameter server to determine Robot model and limb name(s)
    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")
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
    # Parsing Input Arguments
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=valid_limbs,
        help='limb on which to attach joint springs'
        )
    args = parser.parse_args(rospy.myargv()[1:])
    # Grabbing Robot-specific parameters for Dynamic Reconfigure
    config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
    config_module = "intera_examples.cfg"
    cfg = importlib.import_module('.'.join([config_module,config_name]))
    # Starting node connection to ROS
    print("Initializing node... ")
    rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
    dynamic_cfg_srv = Server(cfg, lambda config, level: config)
    js = JointSprings(dynamic_cfg_srv, limb=args.limb)
    # register shutdown callback
    rospy.on_shutdown(js.clean_shutdown)
    js.move_to_neutral()
    js.attach_springs() 

def main():
    """
    This is an example script to demonstrate the functionality of the MotionTrajectory
    to read a yaml file. This script reads in the yaml file creates a trajectory object
    and sends the trajectory to the robot.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)

    parser.add_argument(
        "-f",  "--file_name", type=str, required=True,
        help="The name of the yaml file to ready the trajectory from")
    parser.add_argument(
        "--timeout", type=float, default=None,
        help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")

    args = parser.parse_args(rospy.myargv()[1:])


    try:

        rospy.init_node('send_traj_from_file')
        limb = Limb()
        traj = MotionTrajectory(limb = limb)

        read_result=traj.load_yaml_file(args.file_name)

        if not read_result:
            rospy.logerr('Trajectory not successfully read from file')

        result = traj.send_trajectory(timeout=args.timeout)
        if result is None:
            rospy.logerr('Trajectory FAILED to send')
            return

        if result.result:
            rospy.loginfo('Motion controller successfully finished the trajectory!')
        else:
            rospy.logerr('Motion controller failed to complete the trajectory with error %s',
                         result.errorId)
    except rospy.ROSInterruptException:
        rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.') 

def main():
    """RSDK Joint Position Example: File Playback

    Uses Joint Position Control mode to play back a series of
    recorded joint and gripper positions.

    Run the joint_recorder.py example first to create a recording
    file for use with this example. This example uses position
    control to replay the recorded positions in sequence.

    Note: This version of the playback example simply drives the
    joints towards the next position at each time stamp. Because
    it uses Position Control it will not attempt to adjust the
    movement speed to hit set points "on time".
    """
    epilog = """
Related examples:
  joint_recorder.py; joint_trajectory_file_playback.py.
    """
    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(
        '-f', '--file', metavar='PATH', required=True,
        help='path to input file'
    )
    parser.add_argument(
        "-a", "--arm", dest="arm", default=valid_limbs[0],
        choices=valid_limbs,
        help="Arm on which to run the joint position file playback example.")
    parser.add_argument(
        '-l', '--loops', type=int, default=1,
        help='number of times to loop the input file. 0=infinite.'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_joint_position_file_playback")
    print("Getting robot state... ")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example...")

    rospy.on_shutdown(clean_shutdown)

    print("Enabling robot... ")
    rs.enable()

    map_file(args.file, args.arm, args.loops) 

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

    def clean_shutdown():
        print("\nExiting example.")

    rospy.on_shutdown(clean_shutdown)

    rospy.loginfo("Enabling robot...")
    rs.enable()
    map_keyboard(args.limb)
    print("Done.") 

def main():
    """SDK Joint Recorder Example

    Record timestamped joint and gripper positions to a file for
    later play back.

    Run this example while moving the robot's arm and gripper
    to record a time series of joint and gripper positions to a
    new csv file with the provided *filename*. This example can
    be run in parallel with any other example or standalone
    (moving the arms in zero-g mode while pressing the cuff
    buttons to open/close grippers).

    You can later play the movements back using one of the
    *_file_playback examples.
    """
    epilog = """
Related examples:
  joint_position_file_playback.py; joint_trajectory_file_playback.py.
    """
    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(
        '-f', '--file', dest='filename', required=True,
        help='the file name to record to'
    )
    parser.add_argument(
        '-r', '--record-rate', type=int, default=100, metavar='RECORDRATE',
        help='rate at which to record (default: 100)'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sdk_joint_recorder")
    print("Getting robot state... ")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    recorder = JointRecorder(args.filename, args.record_rate)
    rospy.on_shutdown(recorder.stop)

    print("Recording. Press Ctrl-C to stop.")
    recorder.record()

    print("\nDone.") 

def main():
    """RSDK Joint Position Example: Joystick Control

    Use a game controller to control the angular joint positions
    of Baxter'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 Baxter's arms using the joysticks. 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.
    """
    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'
    )
    args = parser.parse_args(rospy.myargv()[1:])

    joystick = None
    if args.joystick == 'xbox':
        joystick = baxter_external_devices.joystick.XboxController()
    elif args.joystick == 'logitech':
        joystick = baxter_external_devices.joystick.LogitechController()
    elif args.joystick == 'ps3':
        joystick = baxter_external_devices.joystick.PS3Controller()
    else:
        parser.error("Unsupported joystick type '%s'" % (args.joystick))

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_position_joystick")
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example.")
        if not init_state:
            print("Disabling robot...")
            rs.disable()
    rospy.on_shutdown(clean_shutdown)

    print("Enabling robot... ")
    rs.enable()

    map_joystick(joystick)
    print("Done.") 

def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )

    args = parser.parse_args(rospy.myargv()[1:])
    limb = args.limb

    rospy.init_node("visual_servo")

    #Bit of cheating: move the robot's arms to a neutral position before running

    dominant_joints=[1.187301128466797, 1.942403170440674, 0.08206797205810547, -0.996704015789795, -0.6734175651123048, 1.0266166411193849, 0.4985437554931641]

    off_joints = [-1.1255584018249511, 1.4522963092712404, 0.6354515406555176, -0.8843399232055664, 0.6327670742797852, 1.2751215284729005, -0.4084223843078614, ]
    

    names = ['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2']
    off_limb = "right"
    if limb == "right": off_limb = "left"
    off_names = [off_limb+"_"+name for name in names]
    
    jointlists = {}
    jointlists[off_limb] = off_joints
    jointlists[limb] = dominant_joints

    for side in [limb, off_limb]:
        limb_if = baxter_interface.Limb(side)
        limb_names = [side+"_"+name for name in names]
        joint_commands = dict(zip(limb_names, jointlists[side] ))
        limb_if.move_to_joint_positions(joint_commands)

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()
    
    iksvc, ns = ik_command.connect_service(limb)

    command = VisualCommand(iksvc, limb)
    command.subscribe()
    command.publish()
    while not rospy.is_shutdown():
        command.handler_pub.publish(command.done)
        command.pub_rate.sleep()