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.
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()
_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
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()
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
_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()
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
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()
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()
wobble() method is called to perform the actual wobbling as discussed above.