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.