Introduction
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.
Interfaces-
- 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
33
34 import rospy
35
36 from std_msgs.msg import (
37 UInt16,
38 )
39
40 import baxter_interface
41
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):
46
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()
63
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)
111
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
116
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
x=a*cos(2*pi*f*t)
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
136
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:])
144
145 print("Initializing node... ")
146 rospy.init_node("rsdk_joint_velocity_wobbler")
147
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()
4
5 print("Done.")
6
7 if __name__ == '__main__':
8 main()
The wobble() method is called to perform the actual wobbling as discussed above.
