Joint Position Waypoints- Code Walkthrough

From sdk-wiki
Revision as of 15:17, 29 July 2014 by Sdkuser (talk | contribs) (Protected "Joint Position Waypoints- Code Walkthrough": Setting Permissions for (almost all) Pages - editable and moveable by all logged in users ([Edit=Allow only autoconfirmed users] (indefinite) [Move=Allow only autoconfirmed users] (indefinite)))
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

Introduction

This example demonstrates the usage of the position controller for simple joint position moves. The main() function calls the record() method which records the waypoints as desired by the user. Then, the playback() method is invoked which demonstrates a smooth motion playback through the recorded joint positions.
Interfaces -

  • Limb.joint_angles()
  • Limb.set_joint_position_speed()
  • Limb.move_to_joint_positions()
  • Navigator.button0_changed()
  • Navigator.button2_changed()

Code Walkthrough

Now, let's break down the code.

33 import argparse
34 import sys
35 
36 import rospy
37 
38 import baxter_interface

This imports the baxter interface for accessing the limb and the gripper class.

41 class Waypoints(object):
42     def __init__(self, limb, speed, accuracy):
43         # Create baxter_interface limb instance
44         self._arm = limb
45         self._limb = baxter_interface.Limb(self._arm)
46 
47         # Parameters which will describe joint position moves
48         self._speed = speed
49         self._accuracy = accuracy
50 
51         # Recorded waypoints
52         self._waypoints = list()
53 
54         # Recording state
55         self._is_recording = False
56 
57         # Verify robot is enabled
58         print("Getting robot state... ")
59         self._rs = baxter_interface.RobotEnable()
60         self._init_state = self._rs.state().enabled
61         print("Enabling robot... ")
62         self._rs.enable()
63 
64         # Create Navigator I/O
65         self._navigator_io = baxter_interface.Navigator(self._arm)

An instance of the limb interface for the side under interest is created. An instance of the interface for the corresponding Navigator is also created. The robot is then enabled. It is important to note that the robot should be enabled in order to control the limb's movement.

67     def _record_waypoint(self, value):
68         """
69         Stores joint position waypoints
70 
71         Navigator 'OK/Wheel' button callback
72         """
73         if value:
74             print("Waypoint Recorded")
75             self._waypoints.append(self._limb.joint_angles())

The joint_angles() method retrieves the current joint angles and they are then appended to the _waypoints list.

77     def _stop_recording(self, value):
78         """
79         Sets is_recording to false
80 
81         Navigator 'Rethink' button callback
82         """
83         # On navigator Rethink button press, stop recording
84         if value:
85             self._is_recording = False

This Rethink button's press event is captured.

 87     def record(self):
 88         """
 89         Records joint position waypoints upon each Navigator 'OK/Wheel' button
 90         press.
 91         """
 92         rospy.loginfo("Waypoint Recording Started")
 93         print("Press Navigator 'OK/Wheel' button to record a new joint "
 94         "joint position waypoint.")
 95         print("Press Navigator 'Rethink' button when finished recording "
 96               "waypoints to begin playback")
 97         # Connect Navigator I/O signals
 98         # Navigator scroll wheel button press
 99         self._navigator_io.button0_changed.connect(self._record_waypoint)
100         # Navigator Rethink button press
101         self._navigator_io.button2_changed.connect(self._stop_recording)
102 
103         # Set recording flag
104         self._is_recording = True
105 
106         # Loop until waypoints are done being recorded ('Rethink' Button Press)
107         while self._is_recording and not rospy.is_shutdown():
108             rospy.sleep(1.0)
109 
110         # We are now done with the navigator I/O signals, disconnecting them
111         self._navigator_io.button0_changed.disconnect(self._record_waypoint)
112         self._navigator_io.button2_changed.disconnect(self._stop_recording)

The navigator 'OK/Wheel' button invokes the _record_waypoint() method on a press event, which records the current joint positions as explained above. Similarly, the navigator 'Rethink' button invokes the _stop_recording method on a press event, which sets the is_recording variable to false.

114     def playback(self):
115         """
116         Loops playback of recorded joint position waypoints until program is
117         exited
118         """
119         rospy.sleep(1.0)
120 
121         rospy.loginfo("Waypoint Playback Started")
122         print("  Press Ctrl-C to stop...")
123 
124         # Set joint position speed ratio for execution
125         self._limb.set_joint_position_speed(self._speed)
126 
127         # Loop until program is exited
128         loop = 0
129         while not rospy.is_shutdown():
130             loop += 1
131             print("Waypoint playback loop #%d " % (loop,))
132             for waypoint in self._waypoints:
133                 if rospy.is_shutdown():
134                     break
135                 self._limb.move_to_joint_positions(waypoint, timeout=20.0,
136                                                    threshold=self._accuracy)
137             # Sleep for a few seconds between playback loops
138             rospy.sleep(3.0)
139 
140         # Set joint position speed back to default
141         self._limb.set_joint_position_speed(0.3)

The set_joint_position_speed() method sets the ratio of max joint speed to use during joint position moves. The method move_to_joint_positions(), moves the joints to the commanded position. It is important to note that there is not trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and goal positions are obtained. They are then published as a joint command message using the set_joint_positions() method. Thus, all the waypoints that were stored are visited along a smooth trajectory.

143     def clean_shutdown(self):
144         print("\nExiting example...")
145         if not self._init_state:
146             print("Disabling robot...")
147             self._rs.disable()
148         return True

On shutdown, the robot is sent to its initial state that was captured.

151 def main():
152     """RSDK Joint Position Waypoints Example
153 
154     Records joint positions each time the navigator 'OK/wheel'
155     button is pressed.
156     Upon pressing the navigator 'Rethink' button, the recorded joint positions
157     will begin playing back in a loop.
158     """
159     arg_fmt = argparse.RawDescriptionHelpFormatter
160     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
161                                      description=main.__doc__)
162     required = parser.add_argument_group('required arguments')
163     required.add_argument(
164         '-l', '--limb', required=True, choices=['left', 'right'],
165         help='limb to record/playback waypoints'
166     )
167     parser.add_argument(
168         '-s', '--speed', default=0.3, type=float,
169         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
170     )
171     parser.add_argument(
172         '-a', '--accuracy',
173         default=baxter_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
174         help='joint position accuracy (rad) at which waypoints must achieve'
175     )
176     args = parser.parse_args(rospy.myargv()[1:])

The side of the limb on which the example is to be demonstrated is captured along with the speed and the accuracy from the command line arguments.

178     print("Initializing node... ")
179     rospy.init_node("rsdk_joint_position_waypoints_%s" % (args.limb,))
180 
181     waypoints = Waypoints(args.limb, args.speed, args.accuracy)
182 
183     # Register clean shutdown
184     rospy.on_shutdown(waypoints.clean_shutdown)
185 
186     # Begin example program
187     waypoints.record()
188     waypoints.playback()
189 
190 if __name__ == '__main__':
191     main()

The node is initialized and an instance of the Waypoints class is created. The user defined waypoints are recorded using the record() method and are played back using the playback() method as explained above.