joint_velocity_puppet.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2015, 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 import argparse
00031 import sys
00032 
00033 import rospy
00034 
00035 from std_msgs.msg import (
00036     UInt16,
00037 )
00038 
00039 import baxter_interface
00040 
00041 from baxter_interface import CHECK_VERSION
00042 
00043 
00044 class Puppeteer(object):
00045 
00046     def __init__(self, limb, amplification=1.0):
00047         """
00048         Puppets one arm with the other.
00049 
00050         @param limb: the control arm used to puppet the other
00051         @param amplification: factor by which to amplify the arm movement
00052         """
00053         puppet_arm = {"left": "right", "right": "left"}
00054         self._control_limb = limb
00055         self._puppet_limb = puppet_arm[limb]
00056         self._control_arm = baxter_interface.limb.Limb(self._control_limb)
00057         self._puppet_arm = baxter_interface.limb.Limb(self._puppet_limb)
00058         self._amp = amplification
00059 
00060         print("Getting robot state... ")
00061         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
00062         self._init_state = self._rs.state().enabled
00063         print("Enabling robot... ")
00064         self._rs.enable()
00065 
00066     def _reset_control_modes(self):
00067         rate = rospy.Rate(100)
00068         for _ in xrange(100):
00069             if rospy.is_shutdown():
00070                 return False
00071             self._control_arm.exit_control_mode()
00072             self._puppet_arm.exit_control_mode()
00073             rate.sleep()
00074         return True
00075 
00076     def set_neutral(self):
00077         """
00078         Sets both arms back into a neutral pose.
00079         """
00080         print("Moving to neutral pose...")
00081         self._control_arm.move_to_neutral()
00082         self._puppet_arm.move_to_neutral()
00083 
00084     def clean_shutdown(self):
00085         print("\nExiting example...")
00086         #return to normal
00087         self._reset_control_modes()
00088         self.set_neutral()
00089         if not self._init_state:
00090             print("Disabling robot...")
00091             self._rs.disable()
00092         return True
00093 
00094     def puppet(self):
00095         """
00096 
00097         """
00098         self.set_neutral()
00099         rate = rospy.Rate(100)
00100 
00101         control_joint_names = self._control_arm.joint_names()
00102         puppet_joint_names = self._puppet_arm.joint_names()
00103 
00104         print ("Puppeting:\n"
00105               "  Grab %s cuff and move arm.\n"
00106               "  Press Ctrl-C to stop...") % (self._control_limb,)
00107         while not rospy.is_shutdown():
00108             cmd = {}
00109             for idx, name in enumerate(puppet_joint_names):
00110                 v = self._control_arm.joint_velocity(
00111                     control_joint_names[idx])
00112                 if name[-2:] in ('s0', 'e0', 'w0', 'w2'):
00113                     v = -v
00114                 cmd[name] = v * self._amp
00115             self._puppet_arm.set_joint_velocities(cmd)
00116             rate.sleep()
00117 
00118 
00119 def main():
00120     """RSDK Joint Velocity Example: Puppet
00121 
00122     Mirrors the joint velocities measured on one arm as commands to
00123     the other arm. Demonstrates the use of Joint Velocity Control mode.
00124 
00125     Run this example, passing the 'puppeteer' limb as an argument,
00126     then move that limb around in zero-g mode to see the joint
00127     velocities mirrored out on the other arm.
00128     """
00129     max_gain = 3.0
00130     min_gain = 0.1
00131 
00132     arg_fmt = argparse.RawDescriptionHelpFormatter
00133     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00134                                      description=main.__doc__)
00135     required = parser.add_argument_group('required arguments')
00136     required.add_argument(
00137         "-l", "--limb", required=True, choices=['left', 'right'],
00138         help="specify the puppeteer limb (the control limb)"
00139     )
00140     parser.add_argument(
00141         "-a", "--amplification", type=float, default=1.0,
00142         help=("amplification to apply to the puppeted arm [%g, %g]"
00143               % (min_gain, max_gain))
00144     )
00145     args = parser.parse_args(rospy.myargv()[1:])
00146     if (args.amplification < min_gain or max_gain < args.amplification):
00147         print("Exiting: Amplification must be between: [%g, %g]" %
00148               (min_gain, max_gain))
00149         return 1
00150 
00151     print("Initializing node... ")
00152     rospy.init_node("rsdk_joint_velocity_puppet")
00153 
00154     puppeteer = Puppeteer(args.limb, args.amplification)
00155     rospy.on_shutdown(puppeteer.clean_shutdown)
00156     puppeteer.puppet()
00157 
00158     print("Done.")
00159     return 0
00160 
00161 if __name__ == '__main__':
00162     sys.exit(main())


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:28