00001
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 rospy, tf
00031 import thread
00032
00033 from std_msgs.msg import Float64
00034 from sensor_msgs.msg import JointState
00035 from math import asin
00036
00037 class ParallelGripperController:
00038 """ A simple controller that operates a single servo parallel jaw gripper. """
00039 def __init__(self):
00040 rospy.init_node("gripper_controller")
00041
00042
00043 self.calib = { 0.0000 : 1.8097, 0.0159: 1.2167, 0.0254 : 0.8997, 0.0381 : 0.4499, 0.042 : 0.1943 }
00044
00045
00046
00047 self.min = rospy.get_param("~min", 0.0)
00048 self.max = rospy.get_param("~max", 0.042)
00049 self.center = rospy.get_param("~center", 512)
00050 self.invert = rospy.get_param("~invert", False)
00051
00052
00053 self.commandPub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
00054 self.br = tf.TransformBroadcaster()
00055
00056
00057 self.width = 0.0
00058
00059
00060 rospy.Subscriber("~command", Float64, self.commandCb)
00061 rospy.Subscriber("joint_states", JointState, self.stateCb)
00062
00063 r = rospy.Rate(15)
00064 while not rospy.is_shutdown():
00065
00066 self.br.sendTransform((0, -self.width/2.0, 0),
00067 tf.transformations.quaternion_from_euler(0, 0, 0),
00068 rospy.Time.now(),
00069 "gripper_left_link",
00070 "gripper_link")
00071 self.br.sendTransform((0, self.width/2.0, 0),
00072 tf.transformations.quaternion_from_euler(0, 0, 0),
00073 rospy.Time.now(),
00074 "gripper_right_link",
00075 "gripper_link")
00076 r.sleep()
00077
00078 def getCommand(self, width):
00079 """ Get servo command for an opening width. """
00080 keys = self.calib.keys(); keys.sort()
00081
00082 low = keys[0];
00083 high = keys[-1]
00084 for w in keys[1:-1]:
00085 if w > low and w < width:
00086 low = w
00087 if w < high and w > width:
00088 high = w
00089
00090 scale = (width-low)/(high-low)
00091 return ((self.calib[high]-self.calib[low])*scale) + self.calib[low]
00092
00093 def getWidth(self, command):
00094 """ Get opening width for a particular servo command. """
00095 reverse_calib = dict()
00096 for k, v in self.calib.items():
00097 reverse_calib[v] = k
00098 keys = reverse_calib.keys(); keys.sort()
00099
00100 low = keys[0]
00101 high = keys[-1]
00102 for c in keys[1:-1]:
00103 if c > low and c < command:
00104 low = c
00105 if c < high and c > command:
00106 high = c
00107
00108 scale = (command-low)/(high-low)
00109 return ((reverse_calib[high]-reverse_calib[low])*scale) + reverse_calib[low]
00110
00111 def commandCb(self, msg):
00112 """ Take an input command of width to open gripper. """
00113
00114 if msg.data > self.max or msg.data < self.min:
00115 rospy.logerr("Command exceeds limits.")
00116 return
00117
00118 self.commandPub.publish( Float64( self.getCommand(msg.data) ) )
00119
00120 def stateCb(self, msg):
00121 """ The callback that listens for joint_states. """
00122 try:
00123 index = msg.name.index("gripper_joint")
00124 except ValueError:
00125 return
00126 self.width = self.getWidth(msg.position[index])
00127
00128 if __name__=="__main__":
00129 try:
00130 ParallelGripperController()
00131 except rospy.ROSInterruptException:
00132 rospy.loginfo("Hasta la Vista...")
00133