gripper_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004   gripper_controller - action based controller for grippers.
00005   Copyright (c) 2011-2014 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, actionlib
00031 import thread
00032 
00033 from control_msgs.msg import GripperCommandAction
00034 from sensor_msgs.msg import JointState
00035 from std_msgs.msg import Float64
00036 from math import asin
00037 
00038 class TrapezoidGripperModel:
00039     """ A simple gripper with two opposing servos to open/close non-parallel jaws. """
00040 
00041     def __init__(self):
00042         # trapezoid model: base width connecting each gripper's rotation point
00043             #              + length of gripper fingers to computation point
00044             #              = compute angles based on a desired width at comp. point
00045         self.pad_width = rospy.get_param('~pad_width', 0.01)
00046         self.finger_length = rospy.get_param('~finger_length', 0.02)
00047         self.min_opening = rospy.get_param('~min_opening', 0.0)
00048         self.max_opening = rospy.get_param('~max_opening', 0.09)
00049         self.center_l = rospy.get_param('~center_left', 0.0)
00050         self.center_r = rospy.get_param('~center_right', 0.0)
00051         self.invert_l = rospy.get_param('~invert_left', False)
00052         self.invert_r = rospy.get_param('~invert_right', False)
00053 
00054         self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
00055         self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
00056 
00057         # publishers
00058         self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64)
00059         self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64)
00060 
00061     def setCommand(self, command):
00062         # check limits
00063         if command.position > self.max_opening or command.position < self.min_opening:
00064             rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
00065                           command.position, self.max_opening, self.min_opening)
00066             return False
00067         # compute angles
00068         angle = asin((command.position - self.pad_width)/(2*self.finger_length))
00069         if self.invert_l:
00070             l = -angle + self.center_l
00071         else:
00072             l = angle + self.center_l
00073         if self.invert_r:
00074             r = angle + self.center_r
00075         else:
00076             r = -angle + self.center_r
00077         # publish msgs
00078         lmsg = Float64(l)
00079         rmsg = Float64(r)
00080         self.l_pub.publish(lmsg)
00081         self.r_pub.publish(rmsg)
00082         return True
00083 
00084     def getPosition(self, js):
00085         left = right = 0
00086         for i in range(len(js.name)):
00087             if js.name[i] == self.left_joint:
00088                 left = js.position[i]
00089             elif js.name[i] == self.right_joint:
00090                 right = js.position[i]
00091         # TODO
00092 
00093         return 0.0
00094 
00095     def getEffort(self, joint_states):
00096         return 1.0
00097 
00098 class ParallelGripperModel:
00099     """ One servo to open/close parallel jaws, typically via linkage. """
00100 
00101     def __init__(self):
00102         self.center = rospy.get_param('~center', 0.0)
00103         self.scale = rospy.get_param('~scale', 1.0)
00104         self.joint = rospy.get_param('~joint', 'gripper_joint')
00105 
00106         # publishers
00107         self.pub = rospy.Publisher(self.joint+'/command', Float64)
00108 
00109     def setCommand(self, command):
00110         self.pub.publish((command.position * self.scale) + self.center)
00111 
00112     def getPosition(self, joint_states):
00113         return 0.0
00114 
00115     def getEffort(self, joint_states):
00116         return 1.0
00117 
00118 
00119 class OneSideGripperModel:
00120     """ Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
00121 
00122     def __init__(self):
00123         self.pad_width = rospy.get_param('~pad_width', 0.01)
00124         self.finger_length = rospy.get_param('~finger_length', 0.02)
00125         self.min_opening = rospy.get_param('~min_opening', 0.0)
00126         self.max_opening = rospy.get_param('~max_opening', 0.09)
00127         self.center = rospy.get_param('~center', 0.0)
00128         self.invert = rospy.get_param('~invert', False)
00129         self.joint = rospy.get_param('~joint', 'gripper_joint')
00130 
00131         # publishers
00132         self.pub = rospy.Publisher(self.joint+'/command', Float64)
00133 
00134     def setCommand(self, command):
00135         """ Take an input command of width to open gripper. """
00136         # check limits
00137         if command.position > self.max_opening or command.position < self.min_opening:
00138             rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
00139                           command.position, self.max_opening, self.min_opening)
00140             return False
00141         # compute angle
00142         angle = asin((command.position - self.pad_width)/(2*self.finger_length))
00143         # publish message
00144         if self.invert:
00145             self.pub.publish(-angle + self.center)
00146         else:
00147             self.pub.publish(angle + self.center)
00148 
00149     def getPosition(self, joint_states):
00150         # TODO
00151         return 0.0
00152 
00153     def getEffort(self, joint_states):
00154         # TODO
00155         return 1.0
00156 
00157 
00158 class GripperActionController:
00159     """ The actual action callbacks. """
00160     def __init__(self):
00161         rospy.init_node('gripper_controller')
00162 
00163         # setup model
00164         try:
00165             model = rospy.get_param('~model')
00166         except:
00167             rospy.logerr('no model specified, exiting')
00168             exit()
00169         if model == 'dualservo':
00170             self.model = TrapezoidGripperModel()
00171         elif model == 'parallel':
00172             self.model = ParallelGripperModel()
00173         elif model == 'singlesided':
00174             self.model = OneSideGripperModel()
00175         else:
00176             rospy.logerr('unknown model specified, exiting')
00177             exit()
00178 
00179         # subscribe to joint_states
00180         rospy.Subscriber('joint_states', JointState, self.stateCb)
00181 
00182         # subscribe to command and then spin
00183         self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
00184         self.server.start()
00185         rospy.spin()
00186 
00187     def actionCb(self, goal):
00188         """ Take an input command of width to open gripper. """
00189         rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
00190         # send command to gripper
00191         self.model.setCommand(goal.command)
00192         # publish feedback
00193         while True:
00194             if self.server.is_preempt_requested():
00195                 self.server.set_preemtped()
00196                 rospy.loginfo('Gripper Controller: Preempted.')
00197                 return
00198             # TODO: get joint position, break when we have reached goal
00199             break
00200         self.server.set_succeeded()
00201         rospy.loginfo('Gripper Controller: Succeeded.')
00202 
00203     def stateCb(self, msg):
00204         self.state = msg
00205 
00206 if __name__=='__main__':
00207     try:
00208         rospy.logwarn("Please use gripper_controller (no .py)")
00209         GripperActionController()
00210     except rospy.ROSInterruptException:
00211         rospy.loginfo('Hasta la Vista...')
00212 


arbotix_controllers
Author(s): Michael Ferguson
autogenerated on Wed Aug 26 2015 10:44:22