$search
00001 #!/usr/bin/env python 00002 00003 """ 00004 parallel_single_servo_controller.py - controls a single-servo parallel-jaw gripper 00005 Copyright (c) 2011 Vanadium Labs LLC. All right reserved. 00006 00007 Redistribution and use in source and binary forms, with or without 00008 modification, are permitted provided that the following conditions are met: 00009 * Redistributions of source code must retain the above copyright 00010 notice, this list of conditions and the following disclaimer. 00011 * 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 * Neither the name of Vanadium Labs LLC nor the names of its 00015 contributors may be used to endorse or promote products derived 00016 from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00019 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00020 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00021 DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, 00022 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00023 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 00024 OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00025 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 00026 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 00027 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00028 """ 00029 00030 import roslib; roslib.load_manifest('arbotix_controllers') 00031 import rospy, tf 00032 import thread 00033 00034 from std_msgs.msg import Float64 00035 from sensor_msgs.msg import JointState 00036 from math import asin 00037 00038 class ParallelGripperController: 00039 """ A simple controller that operates a single servo parallel jaw gripper. """ 00040 def __init__(self): 00041 rospy.init_node("gripper_controller") 00042 00043 # TODO: load calibration data. Form: opening->servo angle 00044 self.calib = { 0.0000 : 1.8097, 0.0159: 1.2167, 0.0254 : 0.8997, 0.0381 : 0.4499, 0.042 : 0.1943 } 00045 #self.calib = { 0.0000 : 866, 0.0159: 750, 0.0254 : 688, 0.0381 : 600, 0.042 : 550 } 00046 00047 # parameters 00048 self.min = rospy.get_param("~min", 0.0) 00049 self.max = rospy.get_param("~max", 0.042) 00050 self.center = rospy.get_param("~center", 512) 00051 self.invert = rospy.get_param("~invert", False) 00052 00053 # publishers 00054 self.commandPub = rospy.Publisher("gripper_joint/command", Float64) 00055 self.br = tf.TransformBroadcaster() 00056 00057 # current width of gripper 00058 self.width = 0.0 00059 00060 # subscribe to command and then spin 00061 rospy.Subscriber("~command", Float64, self.commandCb) 00062 rospy.Subscriber("joint_states", JointState, self.stateCb) 00063 00064 r = rospy.Rate(15) 00065 while not rospy.is_shutdown(): 00066 # output tf 00067 self.br.sendTransform((0, -self.width/2.0, 0), 00068 tf.transformations.quaternion_from_euler(0, 0, 0), 00069 rospy.Time.now(), 00070 "gripper_left_link", 00071 "gripper_link") 00072 self.br.sendTransform((0, self.width/2.0, 0), 00073 tf.transformations.quaternion_from_euler(0, 0, 0), 00074 rospy.Time.now(), 00075 "gripper_right_link", 00076 "gripper_link") 00077 r.sleep() 00078 00079 def getCommand(self, width): 00080 """ Get servo command for an opening width. """ 00081 keys = self.calib.keys(); keys.sort() 00082 # find end points of segment 00083 low = keys[0]; 00084 high = keys[-1] 00085 for w in keys[1:-1]: 00086 if w > low and w < width: 00087 low = w 00088 if w < high and w > width: 00089 high = w 00090 # linear interpolation 00091 scale = (width-low)/(high-low) 00092 return ((self.calib[high]-self.calib[low])*scale) + self.calib[low] 00093 00094 def getWidth(self, command): 00095 """ Get opening width for a particular servo command. """ 00096 reverse_calib = dict() 00097 for k, v in self.calib.items(): 00098 reverse_calib[v] = k 00099 keys = reverse_calib.keys(); keys.sort() 00100 # find end points of segment 00101 low = keys[0] 00102 high = keys[-1] 00103 for c in keys[1:-1]: 00104 if c > low and c < command: 00105 low = c 00106 if c < high and c > command: 00107 high = c 00108 # linear interpolation 00109 scale = (command-low)/(high-low) 00110 return ((reverse_calib[high]-reverse_calib[low])*scale) + reverse_calib[low] 00111 00112 def commandCb(self, msg): 00113 """ Take an input command of width to open gripper. """ 00114 # check limits 00115 if msg.data > self.max or msg.data < self.min: 00116 rospy.logerr("Command exceeds limits.") 00117 return 00118 # compute angle 00119 self.commandPub.publish( Float64( self.getCommand(msg.data) ) ) 00120 00121 def stateCb(self, msg): 00122 """ The callback that listens for joint_states. """ 00123 try: 00124 index = msg.name.index("gripper_joint") 00125 except ValueError: 00126 return 00127 self.width = self.getWidth(msg.position[index]) 00128 00129 if __name__=="__main__": 00130 try: 00131 ParallelGripperController() 00132 except rospy.ROSInterruptException: 00133 rospy.loginfo("Hasta la Vista...") 00134