nextage_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2013, Tokyo Opensource Robotics Kyokai Association
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
00019 #    names of its contributors may be used to endorse or promote products
00020 #    derived from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 # Author: Isaac Isao Saito
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     # The 2nd arg 'object' is passed to work around the issue raised because
00048     # HrpsysConfigurator is "old-style" python class.
00049     # See http://stackoverflow.com/a/18392639/577001
00050     '''
00051     This class holds methods that are specific to Kawada Industries' dual-arm
00052     robot called Nextage Open.
00053     '''
00054     # TODO: Unittest s'il vous plait!
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     # Default digital input groups defined by manufacturer, Kawada, as of
00065     # July 2014. This may change per the robot in the future and in then
00066     # need modified. See also readDinGroup method.
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         # When there's no ros master running, underlining code,
00086         # socket.create_connection more specifically, returns the simplest
00087         # 'error' that is hard to be explicitly caught at exception block.
00088         # See https://github.com/tork-a/rtmros_nextage/issues/262#issue-182487549
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             # ['co', "CollisionDetector"],
00343             # ['sc', "ServoController"],
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             # Set the pose where eefs level with the tabletop by default.
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         #din = self.rh_svc.readDigitalInput()[1];
00407         ## rh_svc.readDigitalInput() returns tuple, of which 1st element is not needed here.
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 


nextage_ros_bridge
Author(s): Isaac Isao Saito , Akio Ochiai
autogenerated on Wed May 15 2019 04:45:36