Joint Velocity Puppet - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


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
33 import rospy
35 from std_msgs.msg import (
36     UInt16,
37 )
39 import baxter_interface
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):
46     def __init__(self, limb, amplification=1.0):
47         """
48         Puppets one arm with the other.
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         """
 97         """
 98         self.set_neutral()
 99         rate = rospy.Rate(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
122     Mirrors the joint velocities measured on one arm as commands to
123     the other arm. Demonstrates the use of Joint Velocity Control mode.
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
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")
122     puppeteer = Puppeteer(args.limb, args.amplification)
123     rospy.on_shutdown(puppeteer.clean_shutdown)
124     puppeteer.puppet()
126     print("Done.")
127     return 0
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.