Head Wobbler - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


This example demonstrates ability to move Baxter's head by nodding and panning its head to random angles for 10 seconds.The code consists of a main() function that initializes the ros node and creates an instance of the Wobbler class. In the __init__() function, an instance of the head interface is created. The wobble() function is then called, which performs a single head nod before panning the head to random angles for 10 seconds.
Interfaces -

  • Head.set_pan(<double>)
  • Head.command_nod()
  • Head.pan()

Code Walkthrough

Now, let's break down the code.

30 import argparse
31 import random
33 import rospy
35 import baxter_interface
37 from baxter_interface import CHECK_VERSION

This imports the baxter interface for accessing the Head class. The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

40  class Wobbler(object):
41     def __init__(self):
42         """
43         'Wobbles' the head
44         """
45         self._done = False
46         self._head = baxter_interface.Head()

The _head is an object of the Head class within the baxter_interface. This creates a subscriber to the topic robot/head/head_state and publishers to the topics robot/head/command_head_pan, robot/head/command_head_nod. The object _head, would be used to control the head nodding and panning.

49         # verify robot is enabled
50         print("Getting robot state... ")
51         self._rs = baxter_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")

baxter_interface.RobotEnable(CHECK_VERSION) checks if the sdk version updated in the settings is compatible with the sdk version loaded on the param server of the Robot and creates an object _rs of the RobotEnable class. The next line sets the _init_state of the robot to its current state. The enable() performs the actual enabling of the robot. It is important to note that the robot should be in enabled state before attempting to move its joints.

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()

This function performs a clean shutdown of the program. The _done variable gets updated once the head finishes its wobbling. Then, the head is moved to its neutral position and checks if the _init_state was in disabled state and the current state is enabled. If so, it resets the robot to the disable state.

69     def set_neutral(self):
70         """
71         Sets the head back into a neutral pose
72         """
73         self._head.set_pan(0.0)

The set_pan function moves the head to the desired joint position. It employs a PID position controller to move the head joint to that position. In order to move the head to the neutral position, the pan is set to 0.

75     def wobble(self):
76         self.set_neutral()
77         """
78         Performs the wobbling
79         """
80         self._head.command_nod()
81         command_rate = rospy.Rate(1)
82         control_rate = rospy.Rate(100)
83         start = rospy.get_time()

This is where the actual wobbling happens. Before the robot starts to wobble its head, it moves its head to the neutral position. The command_nod() performs a single nod of the head by publishing on the robot/head/command_head_nod topic.

86         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
87             angle = random.uniform(-1.5, 1.5)

This retrieves a random floating point number between -1.5 and 1.5. These random angles are generated for 10 seconds.

84             while (not rospy.is_shutdown() and
85                    not (abs(self._head.pan() - angle) <=
86                        baxter_interface.HEAD_PAN_ANGLE_TOLERANCE)):

This ensures that it loops till the current head pan position and the random angle generated above is within the HEAD_PAN_ANGLE_TOLERANCE

89                 self._head.set_pan(angle, speed=30, timeout=0)
90                 control_rate.sleep()
91             command_rate.sleep()

This publishes the corresponding position and the speed to the robot/head/command_head_pan

93         self._done = True
94         rospy.signal_shutdown("Example finished.")

Once the wobbling is completed, the _done variable is updated as True and a shutdown signal is sent.

 97 def main():
 98     """RSDK Head Example: Wobbler
100     Nods the head and pans side-to-side towards random angles.
101     Demonstrates the use of the baxter_interface.Head class.
102     """
103     arg_fmt = argparse.RawDescriptionHelpFormatter
104     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
105                                      description=main.__doc__)
106     parser.parse_args(rospy.myargv()[1:])
108     print("Initializing node... ")
109     rospy.init_node("rsdk_head_wobbler")
111     wobbler = Wobbler()

An object of the Wobbler class is generated and this initializes the head interface, checks for the appropriate version and enables the robot.

112     rospy.on_shutdown(wobbler.clean_shutdown)
113     print("Wobbling... ")
114     wobbler.wobble()
115     print("Done.")
117 if __name__ == '__main__':
118     main()

The actual nodding and panning happens when the wobble() function is called.