joint_position_waypoints.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2014, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 """
00031 Baxter RSDK Joint Position Waypoints Example
00032 """
00033 import argparse
00034 import sys
00035 
00036 import rospy
00037 
00038 import baxter_interface
00039 
00040 
00041 class Waypoints(object):
00042     def __init__(self, limb, speed, accuracy):
00043         # Create baxter_interface limb instance
00044         self._arm = limb
00045         self._limb = baxter_interface.Limb(self._arm)
00046 
00047         # Parameters which will describe joint position moves
00048         self._speed = speed
00049         self._accuracy = accuracy
00050 
00051         # Recorded waypoints
00052         self._waypoints = list()
00053 
00054         # Recording state
00055         self._is_recording = False
00056 
00057         # Verify robot is enabled
00058         print("Getting robot state... ")
00059         self._rs = baxter_interface.RobotEnable()
00060         self._init_state = self._rs.state().enabled
00061         print("Enabling robot... ")
00062         self._rs.enable()
00063 
00064         # Create Navigator I/O
00065         self._navigator_io = baxter_interface.Navigator(self._arm)
00066 
00067     def _record_waypoint(self, value):
00068         """
00069         Stores joint position waypoints
00070 
00071         Navigator 'OK/Wheel' button callback
00072         """
00073         if value:
00074             print("Waypoint Recorded")
00075             self._waypoints.append(self._limb.joint_angles())
00076 
00077     def _stop_recording(self, value):
00078         """
00079         Sets is_recording to false
00080 
00081         Navigator 'Rethink' button callback
00082         """
00083         # On navigator Rethink button press, stop recording
00084         if value:
00085             self._is_recording = False
00086 
00087     def record(self):
00088         """
00089         Records joint position waypoints upon each Navigator 'OK/Wheel' button
00090         press.
00091         """
00092         rospy.loginfo("Waypoint Recording Started")
00093         print("Press Navigator 'OK/Wheel' button to record a new joint "
00094         "joint position waypoint.")
00095         print("Press Navigator 'Rethink' button when finished recording "
00096               "waypoints to begin playback")
00097         # Connect Navigator I/O signals
00098         # Navigator scroll wheel button press
00099         self._navigator_io.button0_changed.connect(self._record_waypoint)
00100         # Navigator Rethink button press
00101         self._navigator_io.button2_changed.connect(self._stop_recording)
00102 
00103         # Set recording flag
00104         self._is_recording = True
00105 
00106         # Loop until waypoints are done being recorded ('Rethink' Button Press)
00107         while self._is_recording and not rospy.is_shutdown():
00108             rospy.sleep(1.0)
00109 
00110         # We are now done with the navigator I/O signals, disconnecting them
00111         self._navigator_io.button0_changed.disconnect(self._record_waypoint)
00112         self._navigator_io.button2_changed.disconnect(self._stop_recording)
00113 
00114     def playback(self):
00115         """
00116         Loops playback of recorded joint position waypoints until program is
00117         exited
00118         """
00119         rospy.sleep(1.0)
00120 
00121         rospy.loginfo("Waypoint Playback Started")
00122         print("  Press Ctrl-C to stop...")
00123 
00124         # Set joint position speed ratio for execution
00125         self._limb.set_joint_position_speed(self._speed)
00126 
00127         # Loop until program is exited
00128         loop = 0
00129         while not rospy.is_shutdown():
00130             loop += 1
00131             print("Waypoint playback loop #%d " % (loop,))
00132             for waypoint in self._waypoints:
00133                 if rospy.is_shutdown():
00134                     break
00135                 self._limb.move_to_joint_positions(waypoint, timeout=20.0,
00136                                                    threshold=self._accuracy)
00137             # Sleep for a few seconds between playback loops
00138             rospy.sleep(3.0)
00139 
00140         # Set joint position speed back to default
00141         self._limb.set_joint_position_speed(0.3)
00142 
00143     def clean_shutdown(self):
00144         print("\nExiting example...")
00145         if not self._init_state:
00146             print("Disabling robot...")
00147             self._rs.disable()
00148         return True
00149 
00150 
00151 def main():
00152     """RSDK Joint Position Waypoints Example
00153 
00154     Records joint positions each time the navigator 'OK/wheel'
00155     button is pressed.
00156     Upon pressing the navigator 'Rethink' button, the recorded joint positions
00157     will begin playing back in a loop.
00158     """
00159     arg_fmt = argparse.RawDescriptionHelpFormatter
00160     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00161                                      description=main.__doc__)
00162     required = parser.add_argument_group('required arguments')
00163     required.add_argument(
00164         '-l', '--limb', required=True, choices=['left', 'right'],
00165         help='limb to record/playback waypoints'
00166     )
00167     parser.add_argument(
00168         '-s', '--speed', default=0.3, type=float,
00169         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
00170     )
00171     parser.add_argument(
00172         '-a', '--accuracy',
00173         default=baxter_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
00174         help='joint position accuracy (rad) at which waypoints must achieve'
00175     )
00176     args = parser.parse_args(rospy.myargv()[1:])
00177 
00178     print("Initializing node... ")
00179     rospy.init_node("rsdk_joint_position_waypoints_%s" % (args.limb,))
00180 
00181     waypoints = Waypoints(args.limb, args.speed, args.accuracy)
00182 
00183     # Register clean shutdown
00184     rospy.on_shutdown(waypoints.clean_shutdown)
00185 
00186     # Begin example program
00187     waypoints.record()
00188     waypoints.playback()
00189 
00190 if __name__ == '__main__':
00191     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39