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

(Difference between pages)
Jump to: navigation , search
(Edit Joint Postition Ex)
 
(Movement edit)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<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>
+
<span style="font-size:120%;">'''This section describes the SDK example programs. These example programs are provided as a starting point, demonstrating usage of the Python SDK packages for control of your robot.'''</span>
  
 
</div>
 
</div>
  
 
<div class="content-block">
 
<div class="content-block">
== Usage ==
 
  
Start the joint position keyboard example program, ex:
+
== Prerequisites ==
  
<syntaxhighlight lang="bash" enclose="div">
+
* You have completed all of the [[Installation|Installation]] instructions for setting up your robot and development machine.
$ rosrun intera_examples joint_position_keyboard.py
+
* The development workstation has been connected to the robot and [[Hello Robot | connectivity has been validated]].
</syntaxhighlight>
 
 
 
Start the joint position joystick example program, ex:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples joint_position_joystick.py
 
</syntaxhighlight>
 
 
 
Upon startup, you will be prompted with the following:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
Initializing node...
 
Getting robot state...
 
Enabling robot...
 
[INFO] [WallTime: 1399575163.891211] Robot Enabled
 
Controlling joints. Press ? for help, Esc to quit.
 
</syntaxhighlight>
 
  
 
</div>
 
</div>
Line 35: Line 17:
 
<div class="content-block">
 
<div class="content-block">
  
== Arguments ==
+
== Running Examples with the SDK ==
 +
 
 +
=== Initialize ===
  
'''Important Arguments:''' 
+
[[SDK Shell | Initialize your environment]]:
  
See the joint position example available arguments on the command line by passing:
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_position_keyboard.py -h
+
$ cd ~/ros_ws
 +
$ ./intera.sh
 
</syntaxhighlight>
 
</syntaxhighlight>
 +
 +
=== Run a program! ===
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
usage: joint_position_keyboard.py [-h] [-l LIMB]
+
$ rosrun intera_examples <example_program> [arguments]
 
 
RSDK Joint Position Example: Keyboard Control
 
    Use your dev machine's keyboard to control joint positions.
 
    Each key corresponds to increasing or decreasing the angle
 
    of a joint on Sawyer's arm. The increasing and descreasing
 
    are represented by number key and letter key next to the number.
 
   
 
 
 
optional arguments:
 
  -h, --help            show this help message and exit
 
  -l LIMB, --limb LIMB
 
                        Limb on which to run the joint position keyboard example
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
For joint position joystick example:
+
Refer to the individual examples below for a complete list of arguments or run it as
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples joint_position_joystick.py -h
 
</syntaxhighlight>
 
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
usage: joint_position_joystick.py [-h] [-l LIMB] [-j JOYSTICK]
+
$ rosrun intera_examples <example_program> -h
 
 
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>
  
 +
to find the list of arguments.
  
 
</div>
 
</div>
Line 94: Line 46:
 
<div class="content-block">
 
<div class="content-block">
  
== Code Walkthrough ==  
+
== Clone the SDK Examples ==
  
Now, let's break down the code.
+
The <code>'intera_examples'</code> locate under intera_sdk repo, put <code>'intera_sdk'</code> into your ROS development workspace (<code> ~/ros_ws/src </code>). This will allow you to dig into the examples, follow along with the code walkthroughs and base your custom code off of these examples.
  
<syntaxhighlight lang="python" line start="33" enclose="div">
+
<syntaxhighlight lang="bash" enclose="div">
import argparse
+
$ cd ~/ros_ws/src
import sys
+
$ git clone https://github.com/RethinkRobotics/intera_sdk.git
 
 
import rospy
 
 
 
import intera_interface
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the limb and the gripper class.
+
</div>
  
<syntaxhighlight lang="python" line start="41" enclose="div">
+
<div class="content-block">
class Waypoints(object):
 
    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
 
        self._speed = speed
 
        self._accuracy = accuracy
 
  
        # Recorded waypoints
+
== SDK Examples ==
        self._waypoints = list()
 
  
        # Recording state
+
=== Movement ===
        self._is_recording = False
 
  
        # Verify robot is enabled
+
[[Joint Position Waypoint Example]] - Hand-over-hand teach and recording a number of joint position waypoints. These waypoints will then be played back.
        print("Getting robot state... ")
 
        self._rs = intera_interface.RobotEnable()
 
        self._init_state = self._rs.state().enabled
 
        print("Enabling robot... ")
 
        self._rs.enable()
 
  
        # Create Navigator I/O
+
[[Joint Position Example]] - Joystick, keyboard control for Sawyer's arm.
        self._navigator_io = intera_interface.Navigator(self._arm)
 
</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.
+
[[Joint Torque Springs Example]] - Joint torque control example applying virtual spring torques.
  
<syntaxhighlight lang="python" line start="67" enclose="div">
+
[[IK Service Example|Inverse Kinematics Service Example]] - Basic use of Inverse Kinematics solver service.
    def _record_waypoint(self, value):
 
        """
 
        Stores joint position waypoints
 
  
        Navigator 'OK/Wheel' button callback
+
[[Joint Trajectory Playback Example]] - Trajectory playback using the joint trajectory interface.
        """
 
        if value != 'OFF':
 
            print("Waypoint Recorded")
 
            self._waypoints.append(self._limb.joint_angles())
 
</syntaxhighlight>
 
  
The <code>joint_angles()</code> method retrieves the current joint angles and they are then appended to the <code>_waypoints</code> list.
+
[[Head Movement Example]] - Simple demo moving and nodding the head.
  
<syntaxhighlight lang="python" line start="77" enclose="div">
+
[[Gripper Example]] - Joystick and Keyboard control for the grippers.
    def _stop_recording(self, value):
 
        """
 
        Sets is_recording to false
 
  
        Navigator 'Rethink' button callback
+
[[Gripper Cuff Control Example]] - Simple cuff-interaction control with Zero-G mode.
        """
 
        # On navigator Rethink button press, stop recording
 
        if value != 'OFF':
 
            print("Recording Stopped")
 
            self._is_recording = False
 
</syntaxhighlight>
 
  
This Rethink button's press event is captured.
+
=== Robot Configuration ===
  
<syntaxhighlight lang="python" line start="88" enclose="div">
+
[[URDF Configuration Example]] - A simple ROS node that shows how to add segment and joint subtrees to the robot's model.
    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
+
=== Input and Output ===
        self._is_recording = True
 
  
        # Loop until waypoints are done being recorded ('Rethink' Button Press)
+
[[Display Image Example]] - Example tool for displaying image files (png, jpeg) on the Head Screen.
        while not rospy.is_shutdown() and self._is_recording:
 
            rospy.sleep(1.0)
 
  
        # We are now done with the navigator callbacks, disconnecting them
+
[[image_view|View Cameras Example]] - Simple tool for viewing camera feed on development machine.
        self._navigator.deregister_callback(ok_id)
 
        self._navigator.deregister_callback(show_id)
 
</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="115" enclose="div">
 
    def playback(self):
 
        """
 
        Loops playback of recorded joint position waypoints until program is
 
        exited
 
        """
 
        rospy.sleep(1.0)
 
 
 
        rospy.loginfo("Waypoint Playback Started")
 
        print("  Press Ctrl-C to stop...")
 
 
 
        # Set joint position speed ratio for execution
 
        self._limb.set_joint_position_speed(self._speed)
 
 
 
        # Loop until program is exited
 
        loop = 0
 
        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
 
        self._limb.set_joint_position_speed(0.3)
 
</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="144" enclose="div">
 
    def clean_shutdown(self):
 
        print("\nExiting example...")
 
        if not self._init_state:
 
            print("Disabling robot...")
 
            self._rs.disable()
 
        return True
 
</syntaxhighlight>
 
 
 
On shutdown, the robot is sent to its initial state that was captured.
 
 
 
<syntaxhighlight lang="python" line start="152" enclose="div">
 
def main():
 
    """RSDK 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:])
 
</syntaxhighlight>
 
 
 
The speed and accuracy on which the example is to be demonstrated is captured from the command line arguments.
 
 
 
<syntaxhighlight lang="python" line start="178" enclose="div">
 
    print("Initializing node... ")
 
    rospy.init_node("rsdk_joint_position_waypoints")
 
 
 
    waypoints = Waypoints(args.speed, args.accuracy)
 
 
 
    # Register clean shutdown
 
    rospy.on_shutdown(waypoints.clean_shutdown)
 
 
 
    # Begin example program
 
    waypoints.record()
 
    waypoints.playback()
 
 
 
if __name__ == '__main__':
 
    main()
 
</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.
+
[[Input Output Example|I/O Example]] - Flash the lights on the digital outputs.
  
 
</div>
 
</div>

Revision as of 06:16, 27 November 2016

This section describes the SDK example programs. These example programs are provided as a starting point, demonstrating usage of the Python SDK packages for control of your robot.

Prerequisites

Running Examples with the SDK

Initialize

Initialize your environment:

$ cd ~/ros_ws
$ ./intera.sh

Run a program!

$ rosrun intera_examples <example_program> [arguments]

Refer to the individual examples below for a complete list of arguments or run it as

$ rosrun intera_examples <example_program> -h

to find the list of arguments.

Clone the SDK Examples

The 'intera_examples' locate under intera_sdk repo, put 'intera_sdk' into your ROS development workspace ( ~/ros_ws/src ). This will allow you to dig into the examples, follow along with the code walkthroughs and base your custom code off of these examples.

$ cd ~/ros_ws/src
$ git clone https://github.com/RethinkRobotics/intera_sdk.git

SDK Examples

Movement

Joint Position Waypoint Example - Hand-over-hand teach and recording a number of joint position waypoints. These waypoints will then be played back.

Joint Position Example - Joystick, keyboard control for Sawyer's arm.

Joint Torque Springs Example - Joint torque control example applying virtual spring torques.

Inverse Kinematics Service Example - Basic use of Inverse Kinematics solver service.

Joint Trajectory Playback Example - Trajectory playback using the joint trajectory interface.

Head Movement Example - Simple demo moving and nodding the head.

Gripper Example - Joystick and Keyboard control for the grippers.

Gripper Cuff Control Example - Simple cuff-interaction control with Zero-G mode.

Robot Configuration

URDF Configuration Example - A simple ROS node that shows how to add segment and joint subtrees to the robot's model.

Input and Output

Display Image Example - Example tool for displaying image files (png, jpeg) on the Head Screen.

View Cameras Example - Simple tool for viewing camera feed on development machine.

I/O Example - Flash the lights on the digital outputs.