sexy_example_latching.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 # This program is a simple script used for demos. It runs the hand
00020 # through different given poses.
00021 
00022 # It is a good example on how to use the latching feature of the
00023 # ros publisher to make sure a data is received even if we don't stream
00024 # the data.
00025 
00026 import roslib; roslib.load_manifest('sr_example')
00027 import rospy
00028 
00029 import time
00030 import math
00031 from std_msgs.msg import Float64
00032 
00033 class SexyExampleLatching(object):
00034 
00035     # type of controller that is running
00036     controller_type = "_position_controller"
00037 
00038     def __init__(self):
00039 
00040         self.hand_publishers = self.create_hand_publishers()
00041 
00042         self.sleep_time = 3.0
00043 
00044     def run(self):
00045         """
00046         Runs the hand through different predefined position in a given order.
00047         """
00048         start = {"FFJ0":0, "FFJ3":0, "FFJ4":0,
00049                  "MFJ0":0, "MFJ3":0, "MFJ4":0,
00050                  "RFJ0":0, "RFJ3":0, "RFJ4":0,
00051                  "LFJ0":0, "LFJ3":0, "LFJ4":0, "LFJ5":0,
00052                  "THJ1":0, "THJ2":0, "THJ3":0, "THJ4":0, "THJ5":0,
00053                  "WRJ1":0, "WRJ2":0 }
00054 
00055         fist = {"FFJ0":180, "FFJ3":90, "FFJ4":0,
00056                 "MFJ0":180, "MFJ3":90, "MFJ4":0,
00057                 "RFJ0":180, "RFJ3":90, "RFJ4":0,
00058                 "LFJ0":180, "LFJ3":90, "LFJ4":0, "LFJ5":0,
00059                 "THJ1":0, "THJ2":0, "THJ3":0, "THJ4":50, "THJ5":-50,
00060                 "WRJ1":0, "WRJ2":0 }
00061 
00062         victory = {"FFJ0":0, "FFJ3":0, "FFJ4":-20,
00063                    "MFJ0":0, "MFJ3":0, "MFJ4":20,
00064                    "RFJ0":180, "RFJ3":90, "RFJ4":-10,
00065                    "LFJ0":180, "LFJ3":90, "LFJ4":-10, "LFJ5":0,
00066                    "THJ1":40, "THJ2":20, "THJ3":0, "THJ4":50, "THJ5":35,
00067                    "WRJ1":0, "WRJ2":0 }
00068 
00069         wave_1 = {"WRJ2":-20}
00070         wave_2 = {"WRJ2":5}
00071 
00072 
00073         self.publish_pose(start)
00074         time.sleep(self.sleep_time)
00075 
00076         self.publish_pose(fist)
00077         time.sleep(self.sleep_time)
00078 
00079         self.publish_pose(start)
00080         time.sleep(self.sleep_time)
00081 
00082         self.publish_pose(victory)
00083         time.sleep(self.sleep_time)
00084 
00085         self.publish_pose(start)
00086         time.sleep(self.sleep_time)
00087 
00088         for i in range(0,4):
00089             self.publish_pose(wave_1)
00090             time.sleep(self.sleep_time)
00091             self.publish_pose(wave_2)
00092             time.sleep(self.sleep_time)
00093 
00094 
00095     def publish_pose(self, pose):
00096         """
00097         Publish a given pose.
00098         """
00099         for joint, pos in pose.iteritems():
00100             self.hand_publishers[joint].publish(math.radians(pos))
00101 
00102     def create_hand_publishers(self):
00103         """
00104         Creates a dictionary of publishers to send the targets to the controllers
00105         on /sh_??j?_mixed_position_velocity_controller/command
00106         or /sh_??j?_position_controller/command
00107         """
00108         hand_pub = {}
00109 
00110         for joint in ["FFJ0", "FFJ3", "FFJ4",
00111                       "MFJ0", "MFJ3", "MFJ4",
00112                       "RFJ0", "RFJ3", "RFJ4",
00113                       "LFJ0", "LFJ3", "LFJ4", "LFJ5",
00114                       "THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
00115                       "WRJ1", "WRJ2" ]:
00116             # Here we initialize the publisher with the latch set to True.
00117             # this will ensure that the hand gets the message, even though we're
00118             # using the messages more as a service (we don't stream the data, we
00119             # just ask the hand to take a given position)
00120             hand_pub[joint] = rospy.Publisher('sh_'+joint.lower() + self.controller_type + '/command', Float64, latch=True)
00121 
00122         return hand_pub
00123 
00124 def main():
00125     rospy.init_node('sexy_example_latching', anonymous = True)
00126     sexy_example = SexyExampleLatching()
00127     sexy_example.run()
00128 
00129 if __name__ == '__main__':
00130     main()


sr_example
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:09:23