Difference between revisions of "Head Movement Example"

Jump to: navigation , search
(Arguments)
(Edit whole page)
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''Please Edit Me!!!'''</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">
  
== Code Walkthrough ==  
+
== Introduction ==  
<span style="font-size:120%;">[[Head Movement Example_Code_Walkthrough | Head Movement Example - Code Walkthrough]]</span>
+
 
 +
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.
  
 
</div>
 
</div>
Line 17: 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">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_interface robot_enable.py
+
$ rosrun intera_examples head_wobbler.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 +
Sawyer's head will begin panning left and right to random angles.
 +
 +
</div>
  
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.
+
<div class="content-block">
  
For example, run the example with 0.1 speed ratio and 0.1 accuracy:
+
== Code Walkthrough ==
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples joint_position_waypoints.py -s 0.1 -a 0.1
 
</syntaxhighlight>
 
  
 +
Now, let's break down the code.
  
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
+
<syntaxhighlight lang="python" line start="30" enclose="div">
 +
import argparse
 +
import random
  
 +
import rospy
  
You will then be prompted with instructions for recording joint position waypoints.
+
import intera_interface
  
<syntaxhighlight lang="bash" enclose="div">
+
from intera_interface import CHECK_VERSION
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>
 
</syntaxhighlight>
  
 +
This imports the intera interface for accessing the head class.
  
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
+
<syntaxhighlight lang="python" line start="40" enclose="div">
 +
class Wobbler(object):
  
 +
    def __init__(self):
 +
        """
 +
        'Wobbles' the head
 +
        """
 +
        self._done = False
 +
        self._head = intera_interface.Head()
 +
</syntaxhighlight>
  
You will get the feedback that the joint position waypoint has been recorded:
+
<syntaxhighlight lang="python" line start="49" enclose="div">
 +
        # verify robot is enabled
 +
        print("Getting robot state... ")
 +
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
 +
        self._init_state = self._rs.state().enabled
 +
        print("Enabling robot... ")
 +
        self._rs.enable()
 +
        print("Running. Ctrl-c to quit")
 +
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
<syntaxhighlight lang="python" line start="57" enclose="div">
Waypoint Recorded
+
    def clean_shutdown(self):
 +
        """
 +
        Exits example cleanly by moving head to neutral position and
 +
        maintaining start state
 +
        """
 +
        print("\nExiting example...")
 +
        if self._done:
 +
            self.set_neutral()
 +
        if not self._init_state and self._rs.state().enabled:
 +
            print("Disabling robot...")
 +
            self._rs.disable()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 +
<syntaxhighlight lang="python" line start="69" enclose="div">
 +
    def set_neutral(self):
 +
        """
 +
        Sets the head back into a neutral pose
 +
        """
 +
        self._head.set_pan(0.0)
 +
</syntaxhighlight>
  
When done recording waypoints, pressing the [[Hardware_Components#Navigator | navigator's]] 'Rethink' button will begin playback.
+
<syntaxhighlight lang="python" line start="75" enclose="div">
 +
    def wobble(self):
 +
        self.set_neutral()
 +
        """
 +
        Performs the wobbling
 +
        """
 +
        command_rate = rospy.Rate(1)
 +
        control_rate = rospy.Rate(100)
 +
        start = rospy.get_time()
 +
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
<syntaxhighlight lang="python" line start="83" enclose="div">
[INFO] [WallTime: 1399571721.540300] Waypoint Playback Started
+
        while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
  Press Ctrl-C to stop...
+
            angle = random.uniform(-2.0, 0.95)
 
</syntaxhighlight>
 
</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="python" line start="85" enclose="div">
 +
            while (not rospy.is_shutdown() and
 +
                  not (abs(self._head.pan() - angle) <=
 +
                      intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
 +
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
<syntaxhighlight lang="python" line start="88" enclose="div">
Waypoint playback loop #1
+
                self._head.set_pan(angle, speed=0.3, timeout=0)
Waypoint playback loop #2
+
                control_rate.sleep()
...
+
            command_rate.sleep()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Pressing <code> Control-C </code> at any time will stop playback and exit the example.
+
<syntaxhighlight lang="python" line start="92" enclose="div">
 
+
        self._done = True
</div>
+
        rospy.signal_shutdown("Example finished.")
 
+
</syntaxhighlight>
<div class="content-block">
 
  
== Arguments ==
+
<syntaxhighlight lang="python" line start="96" enclose="div">
 +
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:])
  
=== Important Arguments: ===
+
    print("Initializing node... ")
 +
    rospy.init_node("rsdk_head_wobbler")
  
<code> -s </code> or <code> --speed </code>: The joint position motion speed ratio [0.0-1.0] (default:= 0.3)
+
    wobbler = Wobbler()
 
 
<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">
 
$ rosrun baxter_examples joint_position_waypoints.py -h
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
<syntaxhighlight lang="python" line start="111" enclose="div">
usage: joint_position_waypoints.py [-h] [-s SPEED] [-a ACCURACY]
+
    rospy.on_shutdown(wobbler.clean_shutdown)
 
+
     print("Wobbling... ")
RSDK Joint Position Waypoints Example
+
     wobbler.wobble()
 
+
    print("Done.")
     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
 
  
 +
if __name__ == '__main__':
 +
    main()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 
</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()