Wobbler - Code Walkthrough

From sdk-wiki
Revision as of 21:09, 7 January 2015 by Imcmahon (talk | contribs) (Code Walkthrough)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search


This example demonstrates the usage of the velocity controller to control baxter's limbs using the velocity commands. The code consists of a main() function that initializes the ros node and creates an instance of the Wobbler class. In its __init__() function, instances for the left and right limb class are created. The wobble method is called which in turn calls the set_neutral method that sets the limbs to a predefined neutral position. The wobble() method is primarily responsible for calculating the torques necessary for the harmonic motion of the joints. Terminating the example would invoke the clean_shutdown() method where the limbs are sent back to the neutral mode and the original state of the robot is restored.

  • Limb.joint_names()
  • Limb.exit_control_mode()
  • Limb.move_to_neutral()
  • Limb.set_joint_velocities(<double>)

Code Walkthrough

Now, let's break down the code

30 import argparse
31 import math
32 import random
34 import rospy
36 from std_msgs.msg import (
37     UInt16,
38 )
40 import baxter_interface
42 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.

45 class Wobbler(object):
47     def __init__(self):
48         """
49         'Wobbles' both arms by commanding joint velocities sinusoidally.
50         """
51         self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
52                                          UInt16, queue_size=10)
53         self._left_arm = baxter_interface.limb.Limb("left")
54         self._right_arm = baxter_interface.limb.Limb("right")
55         self._left_joint_names = self._left_arm.joint_names()
56         self._right_joint_names = self._right_arm.joint_names()

The _pub_rate publisher sets the frequency at which the joint commands are updated. Two instances of the limb class are created for each of the limbs. The joint_names() method returns an array of strings that hold the names of each of the joints in that limb.

58         print("Getting robot state... ")
59         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
60         self._init_state = self._rs.state().enabled
61         print("Enabling robot... ")
62         self._rs.enable()
64         # set joint state publishing to 100Hz
65         self._pub_rate.publish(100)

baxter_interface.RobotEnable(CHECK_VERSION) checks if the sdk version updated in the settings is the same as 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. The joint state publishing rate is set to be at 100 Hz.

67     def _reset_control_modes(self):
68         rate = rospy.Rate(100)
69         for _ in xrange(100):
70             if rospy.is_shutdown():
71                 return False
72             self._left_arm.exit_control_mode()
73             self._right_arm.exit_control_mode()
74             self._pub_rate.publish(100)
75             rate.sleep()
76         return True

The exit_control_mode() method switches the control from advanced control modes like velocity and torque control to the position control mode. In this example, it would be switching from velocity control mode to position control mode

78     def set_neutral(self):
79         """
80         Sets both arms back into a neutral pose.
81         """
82         print("Moving to neutral pose...")
83         self._left_arm.move_to_neutral()
84         self._right_arm.move_to_neutral()

The move_to_neutral() method moves the set of joints in that limb to a predefined position referred to as its neutral position. This employs the PID position controller to move the joints to that position.

86     def clean_shutdown(self):
87         print("\nExiting example...")
88         #return to normal
89         self._reset_control_modes()
90         self.set_neutral()
91         if not self._init_state:
92             print("Disabling robot...")
93             self._rs.disable()
94         return True

The _reset_control_modes() method and the set_neutral() method switches from velocity control mode to the position control mode and sets the limbs to a neutral position, as explained above. It checks if the robot was initially in disabled state before running the example and sends it back to that state if so.

 96     def wobble(self):
 97         self.set_neutral()
 98         """
 99         Performs the wobbling of both arms.
100         """
101         rate = rospy.Rate(100)
102         start = rospy.Time.now()

The wobble() method is called to perform the actual wobbling of the limbs. The hands are first set to their neutral position.

104         def make_v_func():
105             """
106             returns a randomly parameterized cosine function to control a
107             specific joint.
108             """
109             period_factor = random.uniform(0.3, 0.5)
110             amplitude_factor = random.uniform(0.1, 0.2)
112             def v_func(elapsed):
113                 w = period_factor * elapsed.to_sec()
114                 return amplitude_factor * math.cos(w * 2 * math.pi)
115             return v_func
117         v_funcs = [make_v_func() for _ in self._right_joint_names]

In Line # 117, make_v_func() is called for the number of joint_names in right limb. The left or right limb can be used for this purpose, since both the limbs have an equal number of joints. The period and amplitude are random values generated for each joint. return v_func() indicates that a handle of the method v_func() is generated for this particular joint with the period_factor and amplitude_factor holding the generated random values. Similarly for each joint, a handle of the v_func() method is created with a constant and random amplitude and period values. This is done to make sure that the amplitude and frequency is same for each joint, till the end of the program. Then, the cosine function is calculated for each joint based on the formula

   where t refers to the time elapsed from the start.
         a refers to the amplitude
         f refers to the frequency
119         def make_cmd(joint_names, elapsed):
120             return dict([(joint, v_funcs[i](elapsed))
121                          for i, joint in enumerate(joint_names)])

v_funcs is a list that holds the velocity commands for all the joints in the same order. For each of the joints in the particular limb, the corresponding velcoity command is stored in a dictionary.

124         print("Wobbling. Press Ctrl-C to stop...")
125         while not rospy.is_shutdown():
126             self._pub_rate.publish(100)
127             elapsed = rospy.Time.now() - start
128             cmd = make_cmd(self._left_joint_names, elapsed)
129             self._left_arm.set_joint_velocities(cmd)
130             cmd = make_cmd(self._right_joint_names, elapsed)
131             self._right_arm.set_joint_velocities(cmd)
132             rate.sleep()

At 100 Hz, the velocity commands for the limbs are continuously calculated. set_joint_velcoties() publishes the velocity command as joint command with the mode as 2 to indicate that the commands are to be sent to the velocity controller.

134 def main():
135     """RSDK Joint Velocity Example: Wobbler
137     Commands joint velocities of randomly parameterized cosine waves
138     to each joint. Demonstrates Joint Velocity Control Mode.
139     """
140     arg_fmt = argparse.RawDescriptionHelpFormatter
141     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
142                                      description=main.__doc__)
143     parser.parse_args(rospy.myargv()[1:])
145     print("Initializing node... ")
146     rospy.init_node("rsdk_joint_velocity_wobbler")
148     wobbler = Wobbler()

The main captures the side of limb on which the user wishes to run the example. An instance of the Wobbler class is created for each of the limbs.

1     wobbler = Wobbler()
2     rospy.on_shutdown(wobbler.clean_shutdown)
3     wobbler.wobble()
5     print("Done.")
7 if __name__ == '__main__':
8     main()

The wobble() method is called to perform the actual wobbling as discussed above.