Difference between pages "Joint Position Example" and "Head Movement Example"

(Difference between pages)
Jump to: navigation , search
(/ * Edit move code walkthrough to same page */)
 
(Edit whole page)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''The joint position waypoints example demonstrates basic joint position control. A joint position waypoint is a configuration of the arm in joint space (ie. simultaneous joint angles for all of the arm's seven degrees of freedom). In this example, we enable the robot moving the specified limb in zero-g mode. Interacting with the arm's navigator buttons, we can record a sequence of joint position waypoints. Upon completion of recording, we will continuously command the limb loop through the recorded joint sequence.'''</span>
+
<span style="font-size:120%;">'''The Head "Wobbler" Example randomly moves the head to demonstrate using the head pan .'''</span>
  
 
</div>
 
</div>
Line 8: Line 8:
 
<div class="content-block">
 
<div class="content-block">
  
== Introduction ==
+
== Introduction ==  
  
This example demonstrates the usage of the position controller for simple joint position moves. The <code>main()</code> function calls the <code>record()</code> method which records the waypoints as desired by the user. Then, the <code>playback()</code> method is invoked which demonstrates a smooth motion playback through the recorded joint positions.
+
Sawyer's head can rotate left-to-right in pan mode. The pan motion swings Sawyer's 'face' (a.k.a. screen) to a settable angle, rotating around the vertical axes of the head.
<br />
+
 
'''Interfaces - '''
+
The head wobbler example is intented to be a simple demo whose code can be examined to learn how to use the head and demonstrate the use of the intera_interface Head class.
<div style="column-count:3;-moz-column-count:3;-webkit-column-count:3">
 
* Limb.move_to_joint_positions()
 
* Limb.set_joint_position_speed()
 
* Limb.joint_angles()
 
* Navigator.wheel()
 
* Navigator.button_state()
 
</div>
 
  
 
</div>
 
</div>
Line 27: Line 20:
 
== Usage ==
 
== Usage ==
  
Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], ex:
+
From an RSDK Shell, run the head_wobbler.py demo from the intera_examples package:
 
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_interface robot_enable.py
 
</syntaxhighlight>
 
 
 
 
 
Start the joint position waypoints example program, you can specify the joint position motion speed ratio <code>-s</code> or joint position accuracy(rad) <code>-a</code> on which we will run the example.
 
 
 
For example, run the example with 0.1 speed ratio and 0.1 accuracy:
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples joint_position_waypoints.py -s 0.1 -a 0.1
 
</syntaxhighlight>
 
 
 
 
 
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
 
 
 
 
 
You will then be prompted with instructions for recording joint position waypoints.
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
Press Navigator 'OK/Wheel' button to record a new joint joint position waypoint.
 
Press Navigator 'Rethink' button when finished recording waypoints to begin playback.
 
</syntaxhighlight>
 
 
 
 
 
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
 
 
 
 
 
You will get the feedback that the joint position waypoint has been recorded:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
Waypoint Recorded
 
</syntaxhighlight>
 
 
 
 
 
When done recording waypoints, pressing the [[Hardware_Components#Navigator | navigator's]] 'Rethink' button will begin playback.
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
[INFO] [WallTime: 1399571721.540300] Waypoint Playback Started
 
  Press Ctrl-C to stop...
 
</syntaxhighlight>
 
 
 
 
 
The program will begin looping through the recorded joint positions. When the previous joint position command is fully achieved (within the accuracy threshold), the next recorded joint position will be commanded.
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
Waypoint playback loop #1
 
Waypoint playback loop #2
 
...
 
</syntaxhighlight>
 
 
 
Pressing <code> Control-C </code> at any time will stop playback and exit the example.
 
 
 
</div>
 
  
<div class="content-block">
 
 
== Arguments ==
 
 
'''Important Arguments:''' 
 
 
<code> -s </code> or <code> --speed </code>: The joint position motion speed ratio [0.0-1.0] (default:= 0.3)
 
 
<code> -a </code> or <code> --accuracy </code>: The threshold in Radians at which the current joint position command is considered successful before sending the next following joint position command. (default:= 0.008726646)
 
 
See the joint position waypoint's available arguments on the command line by passing joint_position_waypoints.py the <code>-h</code>, help argument:
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_position_waypoints.py -h
+
$ rosrun intera_examples head_wobbler.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
Sawyer's head will begin panning left and right to random angles.
usage: joint_position_waypoints.py [-h] [-s SPEED] [-a ACCURACY]
 
 
 
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.
 
   
 
 
 
optional arguments:
 
  -h, --help            show this help message and exit
 
  -s SPEED, --speed SPEED
 
                        joint position motion speed ratio [0.0-1.0] (default:=
 
                        0.3)
 
  -a ACCURACY, --accuracy ACCURACY
 
                        joint position accuracy (rad) at which waypoints must
 
                        achieve
 
 
 
</syntaxhighlight>
 
  
 
</div>
 
</div>
Line 128: Line 36:
 
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="30" enclose="div">
 
import argparse
 
import argparse
import sys
+
import random
  
 
import rospy
 
import rospy
  
 
import intera_interface
 
import intera_interface
 +
 +
from intera_interface import CHECK_VERSION
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the limb and the gripper class.
+
This imports the intera interface for accessing the head class.
  
<syntaxhighlight lang="python" line start="41" enclose="div">
+
<syntaxhighlight lang="python" line start="40" enclose="div">
class Waypoints(object):
+
class Wobbler(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
+
    def __init__(self):
         self._speed = speed
+
         """
         self._accuracy = accuracy
+
         'Wobbles' the head
 
+
        """
        # Recorded waypoints
+
         self._done = False
         self._waypoints = list()
+
         self._head = intera_interface.Head()
 
+
</syntaxhighlight>
        # Recording state
 
        self._is_recording = False
 
  
         # Verify robot is enabled
+
<syntaxhighlight lang="python" line start="49" enclose="div">
 +
         # verify robot is enabled
 
         print("Getting robot state... ")
 
         print("Getting robot state... ")
         self._rs = intera_interface.RobotEnable()
+
         self._rs = intera_interface.RobotEnable(CHECK_VERSION)
 
         self._init_state = self._rs.state().enabled
 
         self._init_state = self._rs.state().enabled
 
         print("Enabling robot... ")
 
         print("Enabling robot... ")
 
         self._rs.enable()
 
         self._rs.enable()
 
+
         print("Running. Ctrl-c to quit")
         # Create Navigator I/O
 
        self._navigator_io = intera_interface.Navigator(self._arm)
 
 
</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="57" enclose="div">
 
+
     def clean_shutdown(self):
<syntaxhighlight lang="python" line start="67" enclose="div">
 
     def _record_waypoint(self, value):
 
 
         """
 
         """
         Stores joint position waypoints
+
         Exits example cleanly by moving head to neutral position and
 
+
         maintaining start state
         Navigator 'OK/Wheel' button callback
 
 
         """
 
         """
         if value != 'OFF':
+
        print("\nExiting example...")
             print("Waypoint Recorded")
+
        if self._done:
             self._waypoints.append(self._limb.joint_angles())
+
            self.set_neutral()
 +
         if not self._init_state and self._rs.state().enabled:
 +
             print("Disabling robot...")
 +
             self._rs.disable()
 
</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="69" enclose="div">
 
+
     def set_neutral(self):
<syntaxhighlight lang="python" line start="77" enclose="div">
 
     def _stop_recording(self, value):
 
 
         """
 
         """
         Sets is_recording to false
+
         Sets the head back into a neutral pose
 
 
        Navigator 'Rethink' button callback
 
 
         """
 
         """
         # On navigator Rethink button press, stop recording
+
         self._head.set_pan(0.0)
        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="75" enclose="div">
 
+
     def wobble(self):
<syntaxhighlight lang="python" line start="88" enclose="div">
+
        self.set_neutral()
     def record(self):
 
 
         """
 
         """
         Records joint position waypoints upon each Navigator 'OK/Wheel' button
+
         Performs the wobbling
        press.
 
 
         """
 
         """
         rospy.loginfo("Waypoint Recording Started")
+
         command_rate = rospy.Rate(1)
         print("Press Navigator 'OK/Wheel' button to record a new joint "
+
         control_rate = rospy.Rate(100)
        "joint position waypoint.")
+
         start = rospy.get_time()
        print("Press Navigator 'Rethink' button when finished recording "
+
</syntaxhighlight>
              "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
+
<syntaxhighlight lang="python" line start="83" enclose="div">
         self._is_recording = True
+
         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
 +
            angle = random.uniform(-2.0, 0.95)
 +
</syntaxhighlight>
  
        # Loop until waypoints are done being recorded ('Rethink' Button Press)
 
        while not rospy.is_shutdown() and self._is_recording:
 
            rospy.sleep(1.0)
 
  
        # We are now done with the navigator callbacks, disconnecting them
+
<syntaxhighlight lang="python" line start="85" enclose="div">
        self._navigator.deregister_callback(ok_id)
+
            while (not rospy.is_shutdown() and
        self._navigator.deregister_callback(show_id)
+
                  not (abs(self._head.pan() - angle) <=
 +
                      intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
 
</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="88" enclose="div">
 
+
                self._head.set_pan(angle, speed=0.3, timeout=0)
<syntaxhighlight lang="python" line start="115" enclose="div">
+
                control_rate.sleep()
    def playback(self):
+
            command_rate.sleep()
        """
 
        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>
 
</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="92" enclose="div">
 
+
        self._done = True
<syntaxhighlight lang="python" line start="144" enclose="div">
+
         rospy.signal_shutdown("Example finished.")
    def clean_shutdown(self):
 
        print("\nExiting example...")
 
         if not self._init_state:
 
            print("Disabling robot...")
 
            self._rs.disable()
 
        return True
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
On shutdown, the robot is sent to its initial state that was captured.
+
<syntaxhighlight lang="python" line start="96" enclose="div">
 
 
<syntaxhighlight lang="python" line start="152" enclose="div">
 
 
def main():
 
def main():
     """RSDK Joint Position Waypoints Example
+
     """RSDK Head Example: Wobbler
 
+
     Nods the head and pans side-to-side towards random angles.
     Records joint positions each time the navigator 'OK/wheel'
+
     Demonstrates the use of the intera_interface.Head class.
    button is pressed.
 
     Upon pressing the navigator 'Rethink' button, the recorded joint positions
 
    will begin playing back in a loop.
 
 
     """
 
     """
 
     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(
+
     parser.parse_args(rospy.myargv()[1:])
        '-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... ")
 
     print("Initializing node... ")
     rospy.init_node("rsdk_joint_position_waypoints")
+
     rospy.init_node("rsdk_head_wobbler")
  
     waypoints = Waypoints(args.speed, args.accuracy)
+
     wobbler = Wobbler()
 +
</syntaxhighlight>
  
    # Register clean shutdown
+
<syntaxhighlight lang="python" line start="111" enclose="div">
     rospy.on_shutdown(waypoints.clean_shutdown)
+
     rospy.on_shutdown(wobbler.clean_shutdown)
 
+
     print("Wobbling... ")
     # Begin example program
+
     wobbler.wobble()
     waypoints.record()
+
     print("Done.")
     waypoints.playback()
 
  
 
if __name__ == '__main__':
 
if __name__ == '__main__':
 
     main()
 
     main()
 
</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 13:06, 24 November 2016

The Head "Wobbler" Example randomly moves the head to demonstrate using the head pan .

Introduction

Sawyer's head can rotate left-to-right in pan mode. The pan motion swings Sawyer's 'face' (a.k.a. screen) to a settable angle, rotating around the vertical axes of the head.

The head wobbler example is intented to be a simple demo whose code can be examined to learn how to use the head and demonstrate the use of the intera_interface Head class.

Usage

From an RSDK Shell, run the head_wobbler.py demo from the intera_examples package:

$ rosrun intera_examples head_wobbler.py

Sawyer's head will begin panning left and right to random angles.

Code Walkthrough

Now, let's break down the code.

30 import argparse
31 import random
32 
33 import rospy
34 
35 import intera_interface
36 
37 from intera_interface import CHECK_VERSION

This imports the intera interface for accessing the head class.

40 class Wobbler(object):
41 
42     def __init__(self):
43         """
44         'Wobbles' the head
45         """
46         self._done = False
47         self._head = intera_interface.Head()
49         # verify robot is enabled
50         print("Getting robot state... ")
51         self._rs = intera_interface.RobotEnable(CHECK_VERSION)
52         self._init_state = self._rs.state().enabled
53         print("Enabling robot... ")
54         self._rs.enable()
55         print("Running. Ctrl-c to quit")
57     def clean_shutdown(self):
58         """
59         Exits example cleanly by moving head to neutral position and
60         maintaining start state
61         """
62         print("\nExiting example...")
63         if self._done:
64             self.set_neutral()
65         if not self._init_state and self._rs.state().enabled:
66             print("Disabling robot...")
67             self._rs.disable()
69     def set_neutral(self):
70         """
71         Sets the head back into a neutral pose
72         """
73         self._head.set_pan(0.0)
75     def wobble(self):
76         self.set_neutral()
77         """
78         Performs the wobbling
79         """
80         command_rate = rospy.Rate(1)
81         control_rate = rospy.Rate(100)
82         start = rospy.get_time()
83         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
84             angle = random.uniform(-2.0, 0.95)


85             while (not rospy.is_shutdown() and
86                    not (abs(self._head.pan() - angle) <=
87                        intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
88                 self._head.set_pan(angle, speed=0.3, timeout=0)
89                 control_rate.sleep()
90             command_rate.sleep()
92         self._done = True
93         rospy.signal_shutdown("Example finished.")
 96 def main():
 97     """RSDK Head Example: Wobbler
 98     Nods the head and pans side-to-side towards random angles.
 99     Demonstrates the use of the intera_interface.Head class.
100     """
101     arg_fmt = argparse.RawDescriptionHelpFormatter
102     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
103                                      description=main.__doc__)
104     parser.parse_args(rospy.myargv()[1:])
105 
106     print("Initializing node... ")
107     rospy.init_node("rsdk_head_wobbler")
108 
109     wobbler = Wobbler()
111     rospy.on_shutdown(wobbler.clean_shutdown)
112     print("Wobbling... ")
113     wobbler.wobble()
114     print("Done.")
115 
116 if __name__ == '__main__':
117     main()