00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import sys
00038
00039 import rospy
00040 from hironx_ros_bridge.hironx_client import HIRONX
00041
00042 from nextage_ros_bridge.iros13_hands import Iros13Hands
00043 from nextage_ros_bridge.hands_05 import Hands05
00044
00045
00046 class NextageClient(HIRONX, object):
00047
00048
00049
00050 '''
00051 This class holds methods that are specific to Kawada Industries' dual-arm
00052 robot called Nextage Open.
00053 '''
00054
00055
00056 ''' Overriding a variable in the superclass to set the arms at higher
00057 positions.'''
00058 OffPose = [[0], [0, 0],
00059 [25, -140, -150, 45, 0, 0],
00060 [-25, -140, -150, -45, 0, 0],
00061 [0, 0, 0, 0],
00062 [0, 0, 0, 0]]
00063
00064
00065
00066
00067 _DI_PORTS_L = [25, 21, 22, 23, 24]
00068 _DI_PORTS_R = [20, 16, 17, 18, 19]
00069
00070 HAND_VER_0_4_2 = '0.4.2'
00071 HAND_VER_0_5_1 = '0.5.1'
00072
00073 use_gazebo = False
00074
00075 def __init__(self):
00076 '''
00077 Do not get confused that there is also a method called
00078 'init' (without trailing underscores) that is succeeded from the
00079 super class as the tradition there.
00080 '''
00081 try:
00082 if rospy.has_param('/gazebo'):
00083 self.use_gazebo = True
00084 print("\033[32m[INFO] Assuming Gazebo Simulator, so do not connect to CORBA systmes\033[0m")
00085
00086
00087
00088
00089 except:
00090 print("ROS Master not found running. If you intended to use it (either for rosbridge or for Gazebo purposes, make sure you run necessary nodes.")
00091
00092 if not self.use_gazebo:
00093 super(NextageClient, self).__init__()
00094 else:
00095 self.configurator_name = "gazebo(Nextage)"
00096 self.set_hand_version(self.HAND_VER_0_5_1)
00097
00098 def init(self, robotname="HiroNX(Robot)0", url=""):
00099 '''
00100 Calls init from its superclass, which tries to connect RTCManager,
00101 looks for ModelLoader, and starts necessary RTC components. Also runs
00102 config, logger.
00103 Also internally calls setSelfGroups().
00104
00105 @type robotname: str
00106 @type url: str
00107 '''
00108 if not self.use_gazebo:
00109 HIRONX.init(self, robotname=robotname, url=url)
00110
00111 def get_hand_version(self):
00112 '''
00113 @rtype: str
00114 '''
00115 if not self._hand_version:
00116 return 'Hand module not set yet.'
00117 else:
00118 return self._hand_version
00119
00120 def set_hand_version(self, version=HAND_VER_0_5_1):
00121 self._hand_version = version
00122 if self.HAND_VER_0_4_2 == self._hand_version:
00123 self._hands = Iros13Hands(self)
00124 elif self.HAND_VER_0_5_1 == self._hand_version:
00125 self._hands = Hands05(self)
00126
00127 def _print_msg_deprecated_dio_methods(self, name_method=None):
00128 '''
00129 @type name_method: str
00130 @param name_method: Name of the method that calls this method if available.
00131 '''
00132 rospy.logerr("You're likely to have called a deprecated method." +
00133 " Use self._hands.{} instead".format(name_method))
00134
00135 def handlight_r(self, is_on=True):
00136 '''
00137 @deprecated: Won't be functional after package version 0.5.1.
00138 Use self._hands.%FUNCTION_NAME% instead.
00139 '''
00140 try:
00141 return self._hands.handlight_r(is_on)
00142 except AttributeError:
00143 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00144
00145 def handlight_l(self, is_on=True):
00146 '''
00147 @deprecated: Won't be functional after package version 0.5.1.
00148 Use self._hands.%FUNCTION_NAME% instead.
00149 '''
00150 try:
00151 return self._hands.handlight_r(is_on)
00152 except AttributeError:
00153 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00154
00155 def handlight_both(self, is_on=True):
00156 '''
00157 @deprecated: Won't be functional after package version 0.5.1.
00158 Use self._hands.%FUNCTION_NAME% instead.
00159 '''
00160 try:
00161 return self._hands.handlight_both(is_on)
00162 except AttributeError:
00163 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00164
00165 def handtool_l_eject(self):
00166 '''
00167 @deprecated: Won't be functional after package version 0.5.1.
00168 Use self._hands.%FUNCTION_NAME% instead.
00169 '''
00170 try:
00171 return self._hands.toolchanger_l_command.execute(
00172 self._hands.toolchanger_l_command.HAND_TOOLCHANGE_OFF)
00173 except AttributeError:
00174 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00175
00176 def handtool_r_eject(self):
00177 '''
00178 @deprecated: Won't be functional after package version 0.5.1.
00179 Use self._hands.%FUNCTION_NAME% instead.
00180 '''
00181 try:
00182 return self._hands.toolchanger_r_command.execute(
00183 self._hands.toolchanger_r_command.HAND_TOOLCHANGE_OFF)
00184 except AttributeError:
00185 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00186
00187 def handtool_l_attach(self):
00188 '''
00189 @deprecated: Won't be functional after package version 0.5.1.
00190 Use self._hands.%FUNCTION_NAME% instead.
00191 '''
00192 try:
00193 return self._hands.toolchanger_l_command.execute(
00194 self._hands.toolchanger_l_command.HAND_TOOLCHANGE_ON)
00195 except AttributeError:
00196 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00197
00198 def handtool_r_attach(self):
00199 '''
00200 @deprecated: Won't be functional after package version 0.5.1.
00201 Use self._hands.%FUNCTION_NAME% instead.
00202 '''
00203 try:
00204 return self._hands.toolchanger_r_command.execute(
00205 self._hands.toolchanger_r_command.HAND_TOOLCHANGE_ON)
00206 except AttributeError:
00207 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00208
00209 def gripper_l_close(self):
00210 '''
00211 @deprecated: Won't be functional after package version 0.5.1.
00212 Use self._hands.%FUNCTION_NAME% instead.
00213 '''
00214 try:
00215 return self._hands.gripper_l_command.execute(
00216 self._hands.gripper_l_command.GRIPPER_CLOSE)
00217 except AttributeError:
00218 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00219
00220 def gripper_r_close(self):
00221 '''
00222 @deprecated: Won't be functional after package version 0.5.1.
00223 Use self._hands.%FUNCTION_NAME% instead.
00224 '''
00225 try:
00226 return self._hands.gripper_r_command.execute(
00227 self._hands.gripper_r_command.GRIPPER_CLOSE)
00228 except AttributeError:
00229 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00230
00231 def gripper_l_open(self):
00232 '''
00233 @deprecated: Won't be functional after package version 0.5.1.
00234 Use self._hands.%FUNCTION_NAME% instead.
00235 '''
00236 try:
00237 return self._hands.gripper_l_command.execute(
00238 self._hands.gripper_r_command.GRIPPER_OPEN)
00239 except AttributeError:
00240 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00241
00242 def gripper_r_open(self):
00243 '''
00244 @deprecated: Won't be functional after package version 0.5.1.
00245 Use self._hands.%FUNCTION_NAME% instead.
00246 '''
00247 try:
00248 return self._hands.gripper_r_command.execute(
00249 self._hands.gripper_r_command.GRIPPER_OPEN)
00250 except AttributeError:
00251 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00252
00253 def airhand_l_drawin(self):
00254 '''
00255 @deprecated: Won't be functional after package version 0.5.1.
00256 Use self._hands.%FUNCTION_NAME% instead.
00257 '''
00258 try:
00259 return self._hands.airhand_l_command.execute(
00260 self._hands.airhand_l_command.AIRHAND_DRAWIN)
00261 except AttributeError:
00262 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00263
00264 def airhand_r_drawin(self):
00265 '''
00266 @deprecated: Won't be functional after package version 0.5.1.
00267 Use self._hands.%FUNCTION_NAME% instead.
00268 '''
00269 try:
00270 return self._hands.airhand_r_command.execute(
00271 self._hands.airhand_r_command.AIRHAND_DRAWIN)
00272 except AttributeError:
00273 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00274
00275 def airhand_l_keep(self):
00276 '''
00277 @deprecated: Won't be functional after package version 0.5.1.
00278 Use self._hands.%FUNCTION_NAME% instead.
00279 '''
00280 try:
00281 return self._hands.airhand_l_command.execute(
00282 self._hands.airhand_l_command.AIRHAND_KEEP)
00283 except AttributeError:
00284 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00285
00286 def airhand_r_keep(self):
00287 '''
00288 @deprecated: Won't be functional after package version 0.5.1.
00289 Use self._hands.%FUNCTION_NAME% instead.
00290 '''
00291 try:
00292 return self._hands.airhand_r_command.execute(
00293 self._hands.airhand_r_command.AIRHAND_KEEP)
00294 except AttributeError:
00295 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00296
00297 def airhand_l_release(self):
00298 '''
00299 @deprecated: Won't be functional after package version 0.5.1.
00300 Use self._hands.%FUNCTION_NAME% instead.
00301 '''
00302 try:
00303 return self._hands.airhand_l_command.execute(
00304 self._hands.airhand_l_command.AIRHAND_RELEASE)
00305 except AttributeError:
00306 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00307
00308 def airhand_r_release(self):
00309 '''
00310 @deprecated: Won't be functional after package version 0.5.1.
00311 Use self._hands.%FUNCTION_NAME% instead.
00312 '''
00313 try:
00314 return self._hands.airhand_r_command.execute(
00315 self._hands.airhand_r_command.AIRHAND_RELEASE)
00316 except AttributeError:
00317 self._print_msg_deprecated_dio_methods(sys._getframe().f_code.co_name)
00318
00319 def initialize_hand_dio(self):
00320 '''
00321 @deprecated: Won't be functional after package version 0.5.1.
00322 Use self._hands.%FUNCTION_NAME% instead.
00323
00324 Reset all DIO channels to "off" state except for toolchanger lockers
00325 (if they are turned off the attached tools will fall).
00326 '''
00327 self._hands.init_dio()
00328
00329 def getRTCList(self):
00330 '''
00331 Overwriting HrpsysConfigurator.getRTCList
00332 Returning predefined list of RT components.
00333 @rtype [[str]]
00334 @return List of available components. Each element consists of a list
00335 of abbreviated and full names of the component.
00336 '''
00337 return [
00338 ['seq', "SequencePlayer"],
00339 ['sh', "StateHolder"],
00340 ['fk', "ForwardKinematics"],
00341 ['el', "SoftErrorLimiter"],
00342
00343
00344 ['ic', "ImpedanceController"],
00345 ['log', "DataLogger"]
00346 ]
00347
00348 def goInitial(self, tm=7, wait=True, init_pose_type=0):
00349 '''
00350 @see: HIRONX.goInitial
00351 '''
00352 if not init_pose_type:
00353
00354 init_pose_type = HIRONX.INITPOS_TYPE_EVEN
00355 return HIRONX.goInitial(self, tm, wait, init_pose_type)
00356
00357 def readDinGroup(self, ports, dumpFlag=True):
00358 '''
00359 Print the currently set values of digital input registry. Print output order is tailored
00360 for the hands' functional group; DIO spec that is disloseable as of 7/17/2014 is:
00361
00362 Left hand:
00363 DI26: Tool changer attached or not.
00364 DI22, 23: Fingers.
00365 DI24, 25: Compliance.
00366
00367 Right hand:
00368 DI21: Tool changer attached or not.
00369 DI17, 18: Fingers.
00370 DI19, 20: Compliance.
00371
00372 Example output, for the right hand:
00373
00374 No hand attached:
00375
00376 In [1]: robot.printDin([20, 16, 17, 18, 19])
00377 DI21 is 0
00378 DI17 is 0
00379 DI18 is 0
00380 DI19 is 0
00381 DI20 is 0
00382 Out[1]: [(20, 0), (16, 0), (17, 0), (18, 0), (19, 0)]
00383
00384 Hand attached, fingers closed:
00385
00386 In [1]: robot.printDin([20, 16, 17, 18, 19])
00387 DI21 is 1
00388 DI17 is 1
00389 DI18 is 0
00390 DI19 is 0
00391 DI20 is 0
00392 Out[1]: [(20, 0), (16, 0), (17, 0), (18, 0), (19, 0)]
00393
00394 @author: Koichi Nagashima
00395 @since: 0.2.16
00396 @type ports: int or [int].
00397 @param dumpFlag: Print each pin if True.
00398 @param ports: A port number or a list of port numbers in D-in registry.
00399 @rtype: [(int, int)]
00400 @return: List of tuples of port and din value. If the arg ports was an int value,
00401 this could be a list with single tuple in it.
00402 '''
00403 if isinstance(ports, int):
00404 ports = [ports];
00405 pass;
00406
00407
00408 din = self.readDigitalInput();
00409 resAry=[];
00410 for port in ports:
00411 res = din[port]
00412 if (dumpFlag): print("DI%02d is %d"%(port+1,res));
00413 resAry.append((port, res));
00414 pass;
00415 return resAry;
00416
00417 def readDinGroupL(self, dumpFlag=True):
00418 return self.readDinGroup(self._DI_PORTS_L, dumpFlag)
00419
00420 def readDinGroupR(self, dumpFlag=True):
00421 return self.readDinGroup(self._DI_PORTS_R, dumpFlag)
00422