joint_velocity_wobbler.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 math
00032 import random
00033 
00034 import rospy
00035 
00036 from std_msgs.msg import (
00037     UInt16,
00038 )
00039 
00040 import baxter_interface
00041 
00042 from baxter_interface import CHECK_VERSION
00043 
00044 
00045 class Wobbler(object):
00046 
00047     def __init__(self):
00048         """
00049         'Wobbles' both arms by commanding joint velocities sinusoidally.
00050         """
00051         self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
00052                                          UInt16, queue_size=10)
00053         self._left_arm = baxter_interface.limb.Limb("left")
00054         self._right_arm = baxter_interface.limb.Limb("right")
00055         self._left_joint_names = self._left_arm.joint_names()
00056         self._right_joint_names = self._right_arm.joint_names()
00057 
00058         # control parameters
00059         self._rate = 500.0  # Hz
00060 
00061         print("Getting robot state... ")
00062         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
00063         self._init_state = self._rs.state().enabled
00064         print("Enabling robot... ")
00065         self._rs.enable()
00066 
00067         # set joint state publishing to 500Hz
00068         self._pub_rate.publish(self._rate)
00069 
00070     def _reset_control_modes(self):
00071         rate = rospy.Rate(self._rate)
00072         for _ in xrange(100):
00073             if rospy.is_shutdown():
00074                 return False
00075             self._left_arm.exit_control_mode()
00076             self._right_arm.exit_control_mode()
00077             self._pub_rate.publish(100)  # 100Hz default joint state rate
00078             rate.sleep()
00079         return True
00080 
00081     def set_neutral(self):
00082         """
00083         Sets both arms back into a neutral pose.
00084         """
00085         print("Moving to neutral pose...")
00086         self._left_arm.move_to_neutral()
00087         self._right_arm.move_to_neutral()
00088 
00089     def clean_shutdown(self):
00090         print("\nExiting example...")
00091         #return to normal
00092         self._reset_control_modes()
00093         self.set_neutral()
00094         if not self._init_state:
00095             print("Disabling robot...")
00096             self._rs.disable()
00097         return True
00098 
00099     def wobble(self):
00100         self.set_neutral()
00101         """
00102         Performs the wobbling of both arms.
00103         """
00104         rate = rospy.Rate(self._rate)
00105         start = rospy.Time.now()
00106 
00107         def make_v_func():
00108             """
00109             returns a randomly parameterized cosine function to control a
00110             specific joint.
00111             """
00112             period_factor = random.uniform(0.3, 0.5)
00113             amplitude_factor = random.uniform(0.1, 0.2)
00114 
00115             def v_func(elapsed):
00116                 w = period_factor * elapsed.to_sec()
00117                 return amplitude_factor * math.cos(w * 2 * math.pi)
00118             return v_func
00119 
00120         v_funcs = [make_v_func() for _ in self._right_joint_names]
00121 
00122         def make_cmd(joint_names, elapsed):
00123             return dict([(joint, v_funcs[i](elapsed))
00124                          for i, joint in enumerate(joint_names)])
00125 
00126         print("Wobbling. Press Ctrl-C to stop...")
00127         while not rospy.is_shutdown():
00128             self._pub_rate.publish(self._rate)
00129             elapsed = rospy.Time.now() - start
00130             cmd = make_cmd(self._left_joint_names, elapsed)
00131             self._left_arm.set_joint_velocities(cmd)
00132             cmd = make_cmd(self._right_joint_names, elapsed)
00133             self._right_arm.set_joint_velocities(cmd)
00134             rate.sleep()
00135 
00136 
00137 def main():
00138     """RSDK Joint Velocity Example: Wobbler
00139 
00140     Commands joint velocities of randomly parameterized cosine waves
00141     to each joint. Demonstrates Joint Velocity Control Mode.
00142     """
00143     arg_fmt = argparse.RawDescriptionHelpFormatter
00144     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00145                                      description=main.__doc__)
00146     parser.parse_args(rospy.myargv()[1:])
00147 
00148     print("Initializing node... ")
00149     rospy.init_node("rsdk_joint_velocity_wobbler")
00150 
00151     wobbler = Wobbler()
00152     rospy.on_shutdown(wobbler.clean_shutdown)
00153     wobbler.wobble()
00154 
00155     print("Done.")
00156 
00157 if __name__ == '__main__':
00158     main()


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