Source Code for Module intera_interface.gripper

  1  # Copyright (c) 2013-2016, Rethink Robotics 
  2  # All rights reserved. 
  3  # 
  4  # Redistribution and use in source and binary forms, with or without 
  5  # modification, are permitted provided that the following conditions are met: 
  6  # 
  7  # 1. Redistributions of source code must retain the above copyright notice, 
  8  #    this list of conditions and the following disclaimer. 
  9  # 2. Redistributions in binary form must reproduce the above copyright 
 10  #    notice, this list of conditions and the following disclaimer in the 
 11  #    documentation and/or other materials provided with the distribution. 
 12  # 3. Neither the name of the Rethink Robotics nor the names of its 
 13  #    contributors may be used to endorse or promote products derived from 
 14  #    this software without specific prior written permission. 
 15  # 
 16  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 17  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 18  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 19  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 20  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 21  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 22  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 23  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 24  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 25  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 26  # POSSIBILITY OF SUCH DAMAGE. 
 27   
 28  import rospy 
 29  import intera_dataflow 
 30  from intera_io import IODeviceInterface 
 31  from intera_core_msgs.msg import IONodeConfiguration 
 32   
 33   
 34 -class Gripper(object): 
 35      """ 
 36      Interface class for a gripper on the Intera Research Robot. 
 37      """ 
 38      MAX_POSITION = 0.041667 
 39      MIN_POSITION = 0.0 
 40      MAX_FORCE = 10.0 
 41      MIN_FORCE = 0.0 
 42      MAX_VELOCITY = 3.0 
 43      MIN_VELOCITY = 0.15 
 44   
 45 -    def __init__(self, side="right", calibrate=True): 
 46          """ 
 47          Constructor. 
 48   
 49          @type side: str 
 50          @param side: robot gripper name 
 51          @type calibrate: bool 
 52          @param calibrate: Attempts to calibrate the gripper when initializing class (defaults True) 
 53          """ 
 54          self.devices = None 
 55          self.name = '_'.join([side, 'gripper']) 
 56          self.ee_config_sub = rospy.Subscriber('/io/end_effector/config', IONodeConfiguration, self._config_callback) 
 57          # Wait for the gripper device status to be true 
 58          intera_dataflow.wait_for( 
 59              lambda: not self.devices is None, timeout=5.0, 
 60              timeout_msg=("Failed to get gripper. No gripper attached on the robot.") 
 61              ) 
 62   
 63          self.gripper_io = IODeviceInterface("end_effector", self.name) 
 64          if self.has_error(): 
 65              self.reboot() 
 66              calibrate = True 
 67          if calibrate and self.is_calibrated(): 
 68              self.calibrate() 
 69   
 70 -    def _config_callback(self, msg): 
 71          """ 
 72          config topic callback 
 73          """ 
 74          if msg.devices != []: 
 75              if str(msg.devices[0].name) == self.name: 
 76                  self.devices = msg 
 77   
 78 -    def reboot(self): 
 79          """ 
 80          Power cycle the gripper, removing calibration information. 
 81   
 82          Basic call to the gripper reboot command. Waits for gripper to return 
 83          ready state but does not clear errors that could occur during boot. 
 84   
 85          @rtype: bool 
 86          @return: True if successfully Rebooted, False otherwise 
 87          """ 
 88          return self.gripper_io.set_signal_value("reboot", True) 
 89   
 90 -    def stop(self): 
 91          """ 
 92          Set the gripper to stop executing command at the current 
 93          position, apply holding force. 
 94   
 95          @rtype: bool 
 96          @return: True if successfully Stopped, False otherwise 
 97          """ 
 98          return self.gripper_io.set_signal_value("go", False) 
 99   
100 -    def start(self): 
101          """ 
102          Set the gripper to start executing command at the current 
103          position, apply holding force. 
104   
105          @rtype: bool 
106          @return: True if successfully Started, False otherwise 
107          """ 
108          return self.gripper_io.set_signal_value("go", True) 
109   
110 -    def open(self, position=MAX_POSITION): 
111          """ 
112          Set the gripper position to open by providing opening position. 
113          @type: float 
114          @param: the postion of gripper in meters 
115   
116          @rtype: bool 
117          @return: True if successfully set Position, False otherwise 
118          """ 
119          return self.gripper_io.set_signal_value("position_m", position) 
120   
121 -    def close(self, position=MIN_POSITION): 
122          """ 
123          Set the gripper position to close by providing closing position. 
124          @type: float 
125          @param: the postion of gripper in meters 
126   
127          @rtype: bool 
128          @return: True if successfully set Position, False otherwise 
129          """ 
130          return self.gripper_io.set_signal_value("position_m", position) 
131   
132 -    def has_error(self): 
133          """ 
134          Returns a bool describing whether the gripper is in an error state. 
135          (True: Error detected, False: No errors) 
136   
137          @rtype: bool 
138          @return: True if in error state, False otherwise 
139          """ 
140          return self.gripper_io.get_signal_value("has_error") 
141   
142 -    def is_ready(self): 
143          """ 
144          Returns bool describing if the gripper ready, i.e. is 
145          calibrated, not busy (as in resetting or rebooting), and 
146          not moving. 
147   
148          @rtype: bool 
149          @return: True if in ready state, False otherwise 
150          """ 
151          return (self.is_calibrated() and not self.has_error() 
152            and not self.is_moving()) 
153   
154 -    def is_moving(self): 
155          """ 
156          Returns bool describing if the gripper is moving. 
157   
158          @rtype: bool 
159          @return: True if in moving state, False otherwise 
160          """ 
161          return self.gripper_io.get_signal_value("is_moving") 
162   
163 -    def is_gripping(self): 
164          """ 
165          Returns bool describing if the gripper is gripping. 
166   
167          @rtype: bool 
168          @return: True if in gripping state, False otherwise 
169          """ 
170          return self.gripper_io.get_signal_value("is_gripping") 
171   
172 -    def is_calibrated(self): 
173          """ 
174          Returns bool describing if the gripper is calibrated. 
175   
176          @rtype: bool 
177          @return: True if successfully calibrated, False otherwise 
178          """ 
179          return self.gripper_io.get_signal_value("is_calibrated") 
180   
181 -    def calibrate(self): 
182          """ 
183          Calibrate the gripper in order to set maximum and 
184          minimum travel distance. 
185   
186          @rtype: bool 
187          @return: True if successfully calibrating, False otherwise 
188          """ 
189          return self.gripper_io.set_signal_value("calibrate", True) 
190   
191 -    def get_position(self): 
192          """ 
193          Returns float describing the gripper position in meters. 
194   
195          @rtype: float 
196          @return: Current Position value in Meters (m) 
197          """ 
198          return self.gripper_io.get_signal_value("position_response_m") 
199   
200 -    def set_position(self, position): 
201          """ 
202          Set the position of gripper. 
203          @type: float 
204          @param: the postion of gripper in meters 
205   
206          @rtype: bool 
207          @return: True if successfully set value, False otherwise 
208          """ 
209          return self.gripper_io.set_signal_value("position_m", position) 
210   
211 -    def set_velocity(self, speed): 
212          """ 
213          Set the velocity at which the gripper position movement will execute. 
214          @type: float 
215          @param: the velocity of gripper in meters per second 
216   
217          @rtype: float 
218          @return: Current Velocity value in Meters / second (m/s) 
219          """ 
220          return self.gripper_io.set_signal_value("speed_mps", speed) 
221   
222 -    def get_force(self): 
223          """ 
224          Returns the force sensed by the gripper in estimated Newtons. 
225   
226          @rtype: float 
227          @return: Current Force value in Newton-Meters (N-m) 
228          """ 
229          return self.gripper_io.get_signal_value("force_response_n") 
230   
231 -    def set_holding_force(self, holding_force): 
232          """ 
233          Set holding force of successful gripper grasp. 
234   
235          Set the force at which the gripper will continue applying after a 
236          position command has completed either from successfully achieving the 
237          commanded position, or by exceeding the moving force threshold. 
238   
239          @type: float 
240          @param: the holding force of gripper in newton 
241   
242          @rtype: bool 
243          @return: True if successfully set value, False otherwise 
244          """ 
245          return self.gripper_io.set_signal_value("holding_force_n", holding_force) 
246   
247 -    def set_object_weight(self, object_weight): 
248          """ 
249          Set the weight of the object in kilograms. 
250          @type: float 
251          @param: the object weight in newton 
252   
253          @rtype: bool 
254          @return: True if successfully set value, False otherwise 
255          """ 
256          return self.gripper_io.set_signal_value("object_kg", object_weight) 
257   
258 -    def set_dead_zone(self, dead_zone): 
259          """ 
260          Set the gripper dead zone describing the position error threshold 
261          where a move will be considered successful. 
262   
263          @type: float 
264          @param: the dead zone of gripper in meters 
265   
266          @rtype: bool 
267          @return: True if successfully set value, False otherwise 
268          """ 
269          return self.gripper_io.set_signal_value("dead_zone_m", dead_zone) 
270