Introduction
This program demonstrates the usage of the velocity controller to move Baxter's limbs using velocity commands. The velocity applied across one of the limb is captured and applied on the other. The main() function invokes the puppet() method, which in turn captures the velocity applied across the control limb and applies the same to the other limb.
Interfaces - Limb::joint_names(), Limb::exit_control_mode(), Limb::move_to_neutral(), Limb::joint_velocities(<double>), Limb::set_joint_velocities(<double>)
Code Walkthrough
Now, let's break down the code.
30 import argparse
31 import sys
32
33 import rospy
34
35 from std_msgs.msg import (
36 UInt16,
37 )
38
39 import baxter_interface
40
41 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.
44 class Puppeteer(object):
45
46 def __init__(self, limb, amplification=1.0):
47 """
48 Puppets one arm with the other.
49
50 @param limb: the control arm used to puppet the other
51 @param amplification: factor by which to amplify the arm movement
52 """
53 puppet_arm = {"left": "right", "right": "left"}
54 self._control_limb = limb
55 self._puppet_limb = puppet_arm[limb]
56 self._control_arm = baxter_interface.limb.Limb(self._control_limb)
57 self._puppet_arm = baxter_interface.limb.Limb(self._puppet_limb)
58 self._amp = amplification
The side of the limb that would be controlled is captured as _control_limb while the other limb is the _puppet_limb. Two instances of the Limb class for both the limbs are created.
60 print("Getting robot state... ")
61 self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
62 self._init_state = self._rs.state().enabled
63 print("Enabling robot... ")
64 self._rs.enable()
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. The Robot is then enabled. It is important to note that the robot has to be enabled before trying to control the robot's limbs.
66 def _reset_control_modes(self):
67 rate = rospy.Rate(100)
68 for _ in xrange(100):
69 if rospy.is_shutdown():
70 return False
71 self._control_arm.exit_control_mode()
72 self._puppet_arm.exit_control_mode()
73 rate.sleep()
74 return True
The default controller is the position controller. So, when the method exit_control_mode() is invoked, the default controllers are switched on from the current controllers.
76 def set_neutral(self):
77 """
78 Sets both arms back into a neutral pose.
79 """
80 print("Moving to neutral pose...")
81 self._control_arm.move_to_neutral()
82 self._puppet_arm.move_to_neutral()
The method move_to_neutral() uses the position controller to move to a pre-defined neutral position. Both the limbs are moved to their neutral positions.
84 def clean_shutdown(self):
85 print("\nExiting example...")
86 #return to normal
87 self._reset_control_modes()
88 self.set_neutral()
89 if not self._init_state:
90 print("Disabling robot...")
91 self._rs.disable()
92 return True
On shutdown initialization, the controllers are reset and sent to their neutral position as explained above. The robot is then sent back to its initial position that was captured.
94 def puppet(self):
95 """
96
97 """
98 self.set_neutral()
99 rate = rospy.Rate(100)
100
101 control_joint_names = self._control_arm.joint_names()
102 puppet_joint_names = self._puppet_arm.joint_names()
The method joint_names() returns the list of joint names associated with that limb. The joint names for the control and puppet limbs are captured.
104 print ("Puppeting:\n"
105 " Grab %s cuff and move arm.\n"
106 " Press Ctrl-C to stop...") % (self._control_limb,)
107 while not rospy.is_shutdown():
108 cmd = {}
109 for idx, name in enumerate(puppet_joint_names):
110 v = self._control_arm.joint_velocity(
111 control_joint_names[idx])
112 if name[-2:] in ('s0', 'e0', 'w0', 'w2'):
113 v = -v
114 cmd[name] = v * self._amp
115 self._puppet_arm.set_joint_velocities(cmd)
116 rate.sleep()
The velocity applied across each joint is captured using the method joint_velocity(). The joints s0, e0, w0 and w2 are the joints responsible for the roll. So, it is necessary that the velocity values applied across these joints on the puppet limb should be a negative value of the corresponding joints on the control limb. This velocity is then amplified to compensate for the damping caused by the environment factors. The set_joint_velocities() method applies the velocity across the corresponding joints.
119 def main():
120 """RSDK Joint Velocity Example: Puppet
121
122 Mirrors the joint velocities measured on one arm as commands to
123 the other arm. Demonstrates the use of Joint Velocity Control mode.
124
125 Run this example, passing the 'puppeteer' limb as an argument,
126 then move that limb around in zero-g mode to see the joint
127 velocities mirrored out on the other arm.
128 """
129 max_gain = 3.0
130 min_gain = 0.1
131
132 arg_fmt = argparse.RawDescriptionHelpFormatter
133 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
134 description=main.__doc__)
135 required = parser.add_argument_group('required arguments')
136 required.add_argument(
137 "-l", "--limb", required=True, choices=['left', 'right'],
138 help="specify the puppeteer limb (the control limb)"
139 )
140 parser.add_argument(
141 "-a", "--amplification", type=float, default=1.0,
142 help=("amplification to apply to the puppeted arm [%g, %g]"
143 % (min_gain, max_gain))
144 )
145 args = parser.parse_args(rospy.myargv()[1:])
146 if (args.amplification < min_gain or max_gain < args.amplification):
147 print("Exiting: Amplification must be between: [%g, %g]" %
148 (min_gain, max_gain))
149 return 1
The amplification as entered by the user is captured and checked if it is within the valid range. The limb that is to be controlled is also stored.
119 print("Initializing node... ")
120 rospy.init_node("rsdk_joint_velocity_puppet")
121
122 puppeteer = Puppeteer(args.limb, args.amplification)
123 rospy.on_shutdown(puppeteer.clean_shutdown)
124 puppeteer.puppet()
125
126 print("Done.")
127 return 0
128
129 if __name__ == '__main__':
130 sys.exit(main())
The node is then initialized. An instance of the Puppeteer class is then created using the user entered amplification and the side of limb that is to be controlled. The Ppppet() method performs the actual puppet motion as explained above.
