hironx_client.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2013, JSK Lab, University of Tokyo
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 JSK Lab, University of Tokyo. 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 import math
00036 import numpy
00037 import os
00038 import time
00039 
00040 import roslib
00041 roslib.load_manifest("hrpsys")
00042 from hrpsys.hrpsys_config import *
00043 import OpenHRP
00044 import OpenRTM_aist
00045 import OpenRTM_aist.RTM_IDL
00046 import rtm
00047 from waitInput import waitInputConfirm, waitInputSelect
00048 
00049 from distutils.version import StrictVersion
00050 
00051 SWITCH_ON = OpenHRP.RobotHardwareService.SWITCH_ON
00052 SWITCH_OFF = OpenHRP.RobotHardwareService.SWITCH_OFF
00053 _MSG_ASK_ISSUEREPORT = 'Your report to ' + \
00054                        'https://github.com/start-jsk/rtmros_hironx/issues ' + \
00055                        'about the issue you are seeing is appreciated.'
00056 _MSG_RESTART_QNX = 'You may want to restart QNX/ControllerBox afterward'
00057 
00058 def delete_module(modname, paranoid=None):
00059     from sys import modules
00060     try:
00061         thismod = modules[modname]
00062     except KeyError:
00063         raise ValueError(modname)
00064     these_symbols = dir(thismod)
00065     if paranoid:
00066         try:
00067             paranoid[:]  # sequence support
00068         except:
00069             raise ValueError('must supply a finite list for paranoid')
00070         else:
00071             these_symbols = paranoid[:]
00072     del modules[modname]
00073     for mod in modules.values():
00074         try:
00075             delattr(mod, modname)
00076         except AttributeError:
00077             pass
00078         if paranoid:
00079             for symbol in these_symbols:
00080                 if symbol[:2] == '__':  # ignore special symbols
00081                     continue
00082                 try:
00083                     delattr(mod, symbol)
00084                 except AttributeError:
00085                     pass
00086 
00087 class HrpsysConfigurator2(HrpsysConfigurator): ## JUST FOR TEST, REMOVE WHEN YOU MERGE
00088     default_frame_name = 'WAIST'
00089 
00090     def _get_geometry(self, method, frame_name=None):
00091         '''!@brief
00092         A method only inteded for class-internal usage.
00093 
00094         @since: 315.12.1 or higher
00095         @type method: A Python function object.
00096         @param method: One of the following Python function objects defined in class HrpsysConfigurator:
00097             [getCurrentPose, getCurrentPosition, getCurrentReference, getCurrentRPY,
00098              getReferencePose, getReferencePosition, getReferenceReference, getReferenceRPY]
00099         @param frame_name str: set reference frame name (available from version 315.2.5)
00100         @rtype: {str: [float]}
00101         @return: Dictionary of the values for each kinematic group.
00102             Example (using getCurrentPosition):
00103 
00104                 [{CHEST_JOINT0: [0.0, 0.0, 0.0]},
00105                  {HEAD_JOINT1: [0.0, 0.0, 0.5694999999999999]},
00106                  {RARM_JOINT5: [0.3255751238415409, -0.18236314012331808, 0.06762452267747099]},
00107                  {LARM_JOINT5: [0.3255751238415409, 0.18236314012331808, 0.06762452267747099]}]
00108         '''
00109         _geometry_methods = ['getCurrentPose', 'getCurrentPosition',
00110                              'getCurrentReference', 'getCurrentRPY',
00111                              'getReferencePose', 'getReferencePosition',
00112                              'getReferenceReference', 'getReferenceRPY']
00113         if method.__name__ not in _geometry_methods:
00114             raise NameError("Passed method {} is not supported.".format(method))
00115         for kinematic_group in self.Groups:
00116             # The last element is usually an eef in each kinematic group,
00117             # although not required so.
00118             eef_name = kinematic_group[1][-1]
00119             try:
00120                 result = method(eef_name, frame_name)
00121             except RuntimeError as e:
00122                 print(str(e))
00123             print("{}: {}".format(eef_name, method(eef_name)))
00124         raise RuntimeError("Since no link name passed, ran it for all"
00125                            " available eefs.")
00126 
00127     def getCurrentPose(self, lname=None, frame_name=None):
00128         '''!@brief
00129         Returns the current physical pose of the specified joint.
00130         cf. getReferencePose that returns commanded value.
00131 
00132         eg.
00133         \verbatim
00134              IN: robot.getCurrentPose('LARM_JOINT5')
00135              OUT: [-0.0017702356144599085,
00136               0.00019034630541264752,
00137               -0.9999984150158207,
00138               0.32556275164378523,
00139               0.00012155879975329215,
00140               0.9999999745367515,
00141                0.0001901314142046251,
00142                0.18236394191140365,
00143                0.9999984257434246,
00144                -0.00012122202968358842,
00145                -0.001770258707652326,
00146                0.07462472659364472,
00147                0.0,
00148                0.0,
00149                0.0,
00150                1.0]
00151         \endverbatim
00152 
00153         @type lname: str
00154         @param lname: Name of the link.
00155         @param frame_name str: set reference frame name (from 315.2.5)
00156         @rtype: list of float
00157         @return: Rotational matrix and the position of the given joint in
00158                  1-dimensional list, that is:
00159         \verbatim
00160                  [a11, a12, a13, x,
00161                   a21, a22, a23, y,
00162                   a31, a32, a33, z,
00163                    0,   0,   0,  1]
00164         \endverbatim
00165         '''
00166         if not lname:
00167             self._get_geometry(self.getReferenceRPY, frame_name)
00168         ####
00169         #### for hrpsys >= 315.2.5, frame_name is supported
00170         ####   default_frame_name is set, call with lname:default_frame_name
00171         ####   frame_name is given, call with lname:frame_name
00172         ####   frame_name is none, call with lname
00173         #### for hrpsys <= 315.2.5, frame_name is not supported
00174         ####   frame_name is given, call with lname with warning / oerror
00175         ####   frame_name is none, call with lname
00176         if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'):                         ### CHANGED
00177             if self.default_frame_name and frame_name is None:
00178                 frame_name = self.default_frame_name
00179             if frame_name and not ':' in lname:
00180                 lname = lname + ':' + frame_name
00181         else: # hrpsys < 315.2.4
00182             if frame_name:
00183                 print('frame_name ('+lname+') is not supported') ### CHANGED
00184         pose = self.fk_svc.getCurrentPose(lname)
00185         if not pose[0]:
00186             raise RuntimeError("Could not find reference : " + lname)
00187         return pose[1].data
00188 
00189     def getReferencePose(self, lname=None, frame_name=None):
00190         '''!@brief
00191         Returns the current commanded pose of the specified joint.
00192         cf. getCurrentPose that returns physical pose.
00193 
00194         @type lname: str
00195         @param lname: Name of the link.
00196         @param frame_name str: set reference frame name (from 315.2.5)
00197         @rtype: list of float
00198         @return: Rotational matrix and the position of the given joint in
00199                  1-dimensional list, that is:
00200         \verbatim
00201                  [a11, a12, a13, x,
00202                   a21, a22, a23, y,
00203                   a31, a32, a33, z,
00204                    0,   0,   0,  1]
00205         \endverbatim
00206         '''
00207         if not lname:
00208             # Requires hrpsys 315.12.1 or higher.
00209             self._get_geometry(self.getReferenceRPY, frame_name)
00210         if StrictVersion(self.fk_version) >= StrictVersion('315.2.5'):                         ### CHANGED
00211             if self.default_frame_name and frame_name is None:
00212                 frame_name = self.default_frame_name
00213             if frame_name and not ':' in lname:
00214                 lname = lname + ':' + frame_name
00215         else: # hrpsys < 315.2.4
00216             if frame_name:
00217                 print('frame_name ('+lname+') is not supported') ### CHANGED
00218         pose = self.fk_svc.getReferencePose(lname)
00219         if not pose[0]:
00220             raise RuntimeError("Could not find reference : " + lname)
00221         return pose[1].data
00222 
00223     def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
00224         '''!@brief
00225         Move the end-effector to the given absolute pose.
00226         All d* arguments are in meter.
00227 
00228         @param gname str: Name of the joint group. Case-insensitive
00229         @param pos list of float: In meter.
00230         @param rpy list of float: In radian.
00231         @param tm float: Second to complete the command.
00232         @param frame_name str: Name of the frame that this particular command
00233                            references to.
00234         @return bool: False if unreachable.
00235         '''
00236         print(gname, frame_name, pos, rpy, tm)
00237         # Same change as https://github.com/fkanehiro/hrpsys-base/pull/1113.
00238         # This method should be removed as part of
00239         # https://github.com/start-jsk/rtmros_hironx/pull/470, once
00240         # https://github.com/fkanehiro/hrpsys-base/pull/1063 resolves.
00241         if gname.upper() not in map (lambda x : x[0].upper(), self.Groups):
00242             print("setTargetPose failed. {} is not available in the kinematic groups. "
00243                   "Check available Groups (by e.g. self.Groups/robot.Groups). ".format(gname))
00244             return False
00245         if StrictVersion(self.seq_version) >= StrictVersion('315.2.5'):                         ### CHANGED
00246             if self.default_frame_name and frame_name is None:
00247                 frame_name = self.default_frame_name
00248             if frame_name and not ':' in gname:
00249                 gname = gname + ':' + frame_name
00250         else: # hrpsys < 315.2.4
00251             if frame_name and not ':' in gname:
00252                 print('frame_name ('+gname+') is not supported') ### CHANGED
00253         result = self.seq_svc.setTargetPose(gname, pos, rpy, tm)
00254         if not result:
00255             print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n"
00256                    + "Currently, returning IK result error\n"
00257                    + "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)"
00258                    + " is not implemented. Patch is welcomed.")
00259         return result
00260 
00261 class HIRONX(HrpsysConfigurator2):
00262     '''
00263     @see: <a href = "https://github.com/fkanehiro/hrpsys-base/blob/master/" +
00264                     "python/hrpsys_config.py">HrpsysConfigurator</a>
00265 
00266     This class holds methods that are specific to Kawada Industries' dual-arm
00267     robot called Hiro.
00268 
00269     For the API doc for the derived methods, please see the parent
00270     class via the link above; nicely formatted api doc web page
00271     isn't available yet (discussed in
00272     https://github.com/fkanehiro/hrpsys-base/issues/268).
00273     '''
00274 
00275     Groups = [['torso', ['CHEST_JOINT0']],
00276               ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']],
00277               ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2',
00278                         'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']],
00279               ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2',
00280                         'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']]]
00281 
00282     '''
00283     For OffPose and _InitialPose, the angles of each joint are listed in the
00284     ordered as defined in Groups variable.'''
00285     OffPose = [[0],
00286                [0, 0],
00287                [25, -139, -157, 45, 0, 0],
00288                [-25, -139, -157, -45, 0, 0],
00289                [0, 0, 0, 0],
00290                [0, 0, 0, 0]]
00291     # With this pose the EEFs level up the tabletop surface.
00292     _InitialPose = [[0], [0, 0],
00293                     [-0.6, 0, -100, 15.2, 9.4, 3.2],
00294                     [0.6, 0, -100, -15.2, 9.4, -3.2],
00295                     [0, 0, 0, 0],
00296                     [0, 0, 0, 0]]
00297     # This pose sets joint angles at the factory initial pose. No danger, but
00298     # no real advange either for in normal usage.
00299     # See https://github.com/start-jsk/rtmros_hironx/issues/107
00300     _InitialPose_Factory = [[0], [0, 0],
00301                             [-0, 0, -130, 0, 0, 0],
00302                             [0, 0, -130, 0, 0, 0],
00303                             [0, 0, 0, 0],
00304                             [0, 0, 0, 0]]
00305     INITPOS_TYPE_EVEN = 0
00306     INITPOS_TYPE_FACTORY = 1
00307 
00308     HandGroups = {'rhand': [2, 3, 4, 5], 'lhand': [6, 7, 8, 9]}
00309 
00310     RtcList = []
00311 
00312     # servo controller (grasper)
00313     sc = None
00314     sc_svc = None
00315 
00316     hrpsys_version = '0.0.0'
00317 
00318     _MSG_IMPEDANCE_CALL_DONE = (" call is done. This does't necessarily mean " +
00319                                "the function call was successful, since not " +
00320                                "all methods internally called return status")
00321 
00322     def init(self, robotname="HiroNX(Robot)0", url=""):
00323         '''
00324         Calls init from its superclass, which tries to connect RTCManager,
00325         looks for ModelLoader, and starts necessary RTC components. Also runs
00326         config, logger.
00327         Also internally calls setSelfGroups().
00328 
00329         @type robotname: str
00330         @type url: str
00331         '''
00332         # reload for hrpsys 315.1.8
00333         print(self.configurator_name + "waiting ModelLoader")
00334         HrpsysConfigurator.waitForModelLoader(self)
00335         print(self.configurator_name + "start hrpsys")
00336 
00337         print(self.configurator_name + "finding RTCManager and RobotHardware")
00338         HrpsysConfigurator.waitForRTCManagerAndRoboHardware(self, robotname=robotname)
00339         print self.configurator_name, "Hrpsys controller version info: "
00340         if self.ms :
00341             print self.configurator_name, "  ms = ", self.ms
00342         if self.ms and self.ms.ref :
00343             print self.configurator_name, "  ref = ", self.ms.ref
00344         if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0:
00345             print self.configurator_name, "  version  = ", self.ms.ref.get_component_profiles()[0].version
00346         if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0 and StrictVersion(self.ms.ref.get_component_profiles()[0].version) < StrictVersion('315.2.0'):
00347             sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hrpsys_315_1_9/hrpsys'))
00348             delete_module('ImpedanceControllerService_idl')
00349             import AbsoluteForceSensorService_idl
00350             import ImpedanceControllerService_idl
00351 
00352         # HrpsysConfigurator.init(self, robotname=robotname, url=url)
00353         self.sensors = self.getSensors(url)
00354 
00355         # all([rtm.findRTC(rn[0], rtm.rootnc) for rn in self.getRTCList()]) # not working somehow...
00356         if set([rn[0] for rn in self.getRTCList()]).issubset(set([x.name() for x in self.ms.get_components()])) :
00357             print(self.configurator_name + "hrpsys components are already created and running")
00358             self.findComps(max_timeout_count=0, verbose=True)
00359         else:
00360             print(self.configurator_name + "no hrpsys components found running. creating now")
00361             self.createComps()
00362 
00363             print(self.configurator_name + "connecting hrpsys components")
00364             self.connectComps()
00365 
00366             print(self.configurator_name + "activating hrpsys components")
00367             self.activateComps()
00368 
00369             print(self.configurator_name + "setup hrpsys logger")
00370             self.setupLogger()
00371 
00372         print(self.configurator_name + "setup joint groups for hrpsys controller")
00373         self.setSelfGroups()
00374 
00375         print(self.configurator_name + '\033[32minitialized successfully\033[0m')
00376 
00377         # set hrpsys_version
00378         try:
00379             self.hrpsys_version = self.fk.ref.get_component_profile().version
00380         except:
00381             print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m')
00382 
00383             pass
00384         self.setSelfGroups()
00385         self.hrpsys_version = self.fk.ref.get_component_profile().version
00386 
00387     def goOffPose(self, tm=7):
00388         '''
00389         Move arms to the predefined (as member variable) pose where robot can
00390         be safely turned off.
00391 
00392         @type tm: float
00393         @param tm: Second to complete.
00394         '''
00395         for i in range(len(self.Groups)):
00396             self.setJointAnglesOfGroup(self.Groups[i][0], self.OffPose[i], tm,
00397                                        wait=False)
00398         for i in range(len(self.Groups)):
00399             self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
00400         self.servoOff(wait=False)
00401 
00402     def goInitial(self, tm=7, wait=True, init_pose_type=0):
00403         '''
00404         Move arms to the predefined (as member variable) "initialized" pose.
00405 
00406         @type tm: float
00407         @param tm: Second to complete.
00408         @type wait: bool
00409         @param wait: If true, other command to the robot's joints wait until
00410                      this command returns (done by running
00411                      SequencePlayer.waitInterpolationOfGroup).
00412         @type init_pose_type: int
00413         @param init_pose_type: 0: default init pose (specified as _InitialPose)
00414                                1: factory init pose (specified as
00415                                   _InitialPose_Factory)
00416         '''
00417         if init_pose_type == self.INITPOS_TYPE_FACTORY:
00418             _INITPOSE = self._InitialPose_Factory
00419         else:
00420             _INITPOSE = self._InitialPose
00421 
00422         ret = True
00423         for i in range(len(self.Groups)):
00424             # radangles = [x/180.0*math.pi for x in self._InitialPose[i]]
00425             print(
00426                 '{}, JntAnglesOfGr={}, INITPOSE[i]={}, tm={}, wait={}'.format(
00427                     self.configurator_name, self.Groups[i][0], _INITPOSE[i],
00428                     tm, wait))
00429             ret &= self.setJointAnglesOfGroup(self.Groups[i][0],
00430                                               _INITPOSE[i],
00431                                               tm, wait=False)
00432         if wait:
00433             for i in range(len(self.Groups)):
00434                 self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
00435         return ret
00436 
00437     def getRTCList(self):
00438         '''
00439         @see: HrpsysConfigurator.getRTCList
00440 
00441         @rtype [[str]]
00442         @rerutrn List of available components. Each element consists of a list
00443                  of abbreviated and full names of the component.
00444         '''
00445         rtclist = [
00446             ['seq', "SequencePlayer"],
00447             ['sh', "StateHolder"],
00448             ['fk', "ForwardKinematics"],
00449             ['ic', "ImpedanceController"],
00450             ['el', "SoftErrorLimiter"],
00451             # ['co', "CollisionDetector"],
00452             ['sc', "ServoController"],
00453             ['log', "DataLogger"],
00454             ]
00455         if hasattr(self, 'rmfo'):
00456             self.ms.load("RemoveForceSensorLinkOffset")
00457             self.ms.load("AbsoluteForceSensor")
00458             if "RemoveForceSensorLinkOffset" in self.ms.get_factory_names():
00459                 rtclist.append(['rmfo', "RemoveForceSensorLinkOffset"])
00460             elif "AbsoluteForceSensor" in self.ms.get_factory_names():
00461                 rtclist.append(['rmfo', "AbsoluteForceSensor"])
00462             else:
00463                 print "Component rmfo is not loadable."
00464         return rtclist
00465 
00466     # hand interface
00467     # effort: 1~100[%]
00468     # hiro.HandOpen("rhand", effort)
00469     # hiro.HandOpen()        # for both hand
00470     # hiro.HandClose("rhand", effort)
00471     # hiro.HandClose()       # for both hand
00472     #
00473     def HandOpen(self, hand=None, effort=None):
00474         '''
00475         Set the stretch between two fingers of the specified hand as
00476         hardcoded value (100mm), by internally calling self.setHandWidth.
00477 
00478         @type hand: str
00479         @param hand: Name of the hand joint group. In the default
00480                      setting of HIRONX, hand joint groups are defined
00481                      in member 'HandGroups' where 'lhand' and 'rhand'
00482                      are added.
00483         @type effort: int
00484         '''
00485         self.setHandWidth(hand, 100, effort=effort)
00486 
00487     def HandClose(self, hand=None, effort=None):
00488         '''
00489         Close 2-finger hand, by internally calling self.setHandWidth
00490         setting 0 width.
00491 
00492         @type hand: str
00493         @param hand: Name of the hand joint group. In the default
00494                      setting of HIRONX, hand joint groups are defined
00495                      in member 'HandGroups' where 'lhand' and 'rhand'
00496                      are added.
00497         @type effort: int
00498         '''
00499         self.setHandWidth(hand, 0, effort=effort)
00500 
00501     def setHandJointAngles(self, hand, angles, tm=1):
00502         '''
00503         @type hand: str
00504         @param hand: Name of the hand joint group. In the default
00505                      setting of HIRONX, hand joint groups are defined
00506                      in member 'HandGroups' where 'lhand' and 'rhand'
00507                      are added.
00508         @type angles: OpenHRP::ServoControllerService::dSequence.
00509         @param angles: List of (TODO: document). Elements are in degree.
00510         @param tm: Time to complete the task.
00511         '''
00512         self.sc_svc.setJointAnglesOfGroup(hand, angles, tm)
00513 
00514     def setHandEffort(self, effort=100):
00515         '''
00516         Set maximum torque for all existing hand components.
00517         @type effort: int
00518         '''
00519 
00520         for i in [v for vs in self.HandGroups.values() for v in vs]:  # flatten
00521             self.sc_svc.setMaxTorque(i, effort)
00522 
00523     def setHandWidth(self, hand, width, tm=1, effort=None):
00524         '''
00525         @type hand: str
00526         @param hand: Name of the hand joint group. In the default
00527                      setting of HIRONX, hand joint groups are defined
00528                      in member 'HandGroups' where 'lhand' and 'rhand'
00529                      are added.
00530         @param width: Max=100.
00531         @param tm: Time to complete the move.
00532         @type effort: int
00533         @param effort: Passed to self.setHandEffort if set. Not set by default.
00534         '''
00535         if effort:
00536             self.setHandEffort(effort)
00537         if hand:
00538             self.setHandJointAngles(hand, self.hand_width2angles(width), tm)
00539         else:
00540             for h in self.HandGroups.keys():
00541                 self.setHandJointAngles(h, self.hand_width2angles(width), tm)
00542 
00543     def moveHand(self, hand, av, tm=1):  # direction av: + for open, - for close
00544         '''
00545         Negate the angle value for {2, 3, 6, 7}th element in av.
00546 
00547         @type hand: str
00548         @param hand: Specifies hand. (TODO: List the possible values. Should be
00549                      listed in setHandJointAngles so just copy from its doc.)
00550         @type av: [int]
00551         @param av: angle of each joint of the specified arm
00552                   (TODO: need verified. Also what's the length of the list?)
00553         @param tm: Time in second to complete the work.
00554         '''
00555         for i in [2, 3, 6, 7]:  # do not change this line if servo is different, change HandGroups
00556             av[i] = -av[i]
00557         self.setHandJointAngles(hand, av, tm)
00558 
00559     def hand_width2angles(self, width):
00560         '''
00561         TODO: Needs documented what this method does.
00562 
00563         @type width: float
00564         @return: None if the given width is invalid.
00565         '''
00566         safetyMargin = 3
00567         l1, l2 = (41.9, 19)  # TODO: What are these variables?
00568 
00569         if width < 0.0 or width > (l1 + l2 - safetyMargin) * 2:
00570             return None
00571 
00572         xPos = width / 2.0 + safetyMargin
00573         a2Pos = xPos - l2
00574         a1radH = math.acos(a2Pos / l1)
00575         a1rad = math.pi / 2.0 - a1radH
00576 
00577         return a1rad, -a1rad, -a1rad, a1rad
00578 
00579     def setSelfGroups(self):
00580         '''
00581         Set to the hrpsys.SequencePlayer the groups of links and joints that
00582         are statically defined as member variables (Groups) within this class.
00583 
00584         That said, override Groups variable if you prefer link and joint
00585         groups set differently.
00586         '''
00587         #TODO: Accept groups variable. The name of the method sounds more
00588         #      natural if it accepts it.
00589         for item in self.Groups:
00590             self.seq_svc.addJointGroup(item[0], item[1])
00591         for k, v in self.HandGroups.iteritems():
00592             if self.sc_svc:
00593                 self.sc_svc.addJointGroup(k, v)
00594 
00595     def isCalibDone(self):
00596         '''
00597         Check whether joints have been calibrated.
00598         @rtype bool
00599         '''
00600         if self.simulation_mode:
00601             return True
00602         else:
00603             rstt = self.rh_svc.getStatus()
00604             for item in rstt.servoState:
00605                 if not item[0] & 1:
00606                     return False
00607         return True
00608 
00609     def isServoOn(self, jname='any'):
00610         '''
00611         Check whether servo control has been turned on. Check is done by
00612         HIRONX.getActualState().servoState.
00613         @type jname: str
00614         @param jname: Name of a link (that can be obtained by "hiro.Groups"
00615                       as lists of groups).
00616 
00617                       Reserved values:
00618                       - any: This command will check all servos available.
00619                       - all: Same as 'any'.
00620         @rtype bool
00621         @return: If jname is specified either 'any' or 'all', return False
00622                  if the control of any of servos isn't available.
00623         '''
00624         if self.simulation_mode:
00625             return True
00626         else:
00627             s_s = self.getActualState().servoState
00628             if jname.lower() == 'any' or jname.lower() == 'all':
00629                 for s in s_s:
00630                     # print self.configurator_name, 's = ', s
00631                     if (s[0] & 2) == 0:
00632                         return False
00633             else:
00634                 jid = eval('self.' + jname)
00635                 print self.configurator_name, s_s[jid]
00636                 if s_s[jid][0] & 1 == 0:
00637                     return False
00638             return True
00639         return False
00640 
00641     def flat2Groups(self, flatList):
00642         '''
00643         @type flatList: [int]
00644         @param flatList: single dimension list, with its length depends on
00645                          'Groups' variable defined within this class. Excessive
00646                          elements will be dropped (see example below in @return)
00647 
00648                          eg. If the number of joints of the robot is 15,
00649                              len(flatList) should be 15.
00650         @rtype: [[]]
00651         @return: 2-dimensional list that has the same format with
00652                  'Groups' variable.
00653 
00654                  eg.
00655                  ipython> flatlist = [0, 10, 20, 30, 40, 50, 60, 70, 80, 90,
00656                                       100, 110, 120, 130, 140, 150]
00657                  ipython> robot.flat2Groups(flatlist)
00658                  [[0], [10, 20],
00659                   [30, 40, 50, 60, 70, 80],
00660                   [90, 100, 110, 120, 130, 140]]
00661 
00662         '''
00663         retList = []
00664         index = 0
00665         for group in self.Groups:
00666             joint_num = len(group[1])
00667             retList.append(flatList[index: index + joint_num])
00668             index += joint_num
00669         return retList
00670 
00671     def servoOn(self, jname='all', destroy=1, tm=3):
00672         '''
00673         Turn on servo motors at joint specified.
00674         Joints need to be calibrated (otherwise error returns).
00675 
00676         *Troubleshooting*
00677         When this method does not seem to function as expected, try the
00678         following first before you report to the developer's community:
00679 
00680         - Manually move the arms to the safe pose where arms do not obstruct
00681           to anything and they can move back to the initial pose by goInitial.
00682           Then run the command again.
00683         - Make sure the emergency switch is toggled back.
00684         - Try running goActual() then servoOn().
00685 
00686         If none of the above did not solve your issue, please report with:
00687         - The result of this command (%ROSDISTRO% is "indigo" as of May 2017):
00688 
00689             Ubuntu$ rosversion hironx_ros_bridge
00690             Ubuntu$ dpkg -p ros-%ROSDISTRO%-hironx-ros-bridge
00691 
00692         @type jname: str
00693         @param jname: The value 'all' works iteratively for all servos.
00694         @param destroy: Not used.
00695         @type tm: float
00696         @param tm: Second to complete.
00697         @rtype: int
00698         @return: 1 or -1 indicating success or failure, respectively.
00699         '''
00700         # check joints are calibrated
00701         if not self.isCalibDone():
00702             waitInputConfirm('!! Calibrate Encoders with checkEncoders first !!\n\n')
00703             return -1
00704 
00705         # check servo state
00706         if self.isServoOn():
00707             return 1
00708 
00709         # check jname is acceptable
00710         if jname == '':
00711             jname = 'all'
00712 
00713         #self.rh_svc.power('all', SWITCH_ON)  #do not switch on before goActual
00714 
00715         try:
00716             waitInputConfirm(
00717                 '!! Robot Motion Warning (SERVO_ON) !!\n\n'
00718                 'Confirm RELAY switched ON\n'
00719                 'Push [OK] to switch servo ON(%s).' % (jname))
00720         except:  # ths needs to change
00721             self.rh_svc.power('all', SWITCH_OFF)
00722             raise
00723 
00724         # Need to reset JointGroups.
00725         # See https://code.google.com/p/rtm-ros-robotics/issues/detail?id=277
00726         try:
00727             # remove jointGroups
00728             self.seq_svc.removeJointGroup("larm")
00729             self.seq_svc.removeJointGroup("rarm")
00730             self.seq_svc.removeJointGroup("head")
00731             self.seq_svc.removeJointGroup("torso")
00732         except:
00733             print(self.configurator_name,
00734                   'Exception during servoOn while removing JoingGroup. ' +
00735                   _MSG_ASK_ISSUEREPORT)
00736         try:
00737             # setup jointGroups
00738             self.setSelfGroups()  # restart groups
00739         except:
00740             print(self.configurator_name,
00741                   'Exception during servoOn while removing setSelfGroups. ' +
00742                   _MSG_ASK_ISSUEREPORT)
00743 
00744         try:
00745             self.goActual()  # This needs to happen before turning servo on.
00746             time.sleep(0.1)
00747             self.rh_svc.servo(jname, SWITCH_ON)
00748             time.sleep(2)
00749             # time.sleep(7)
00750             if not self.isServoOn(jname):
00751                 print self.configurator_name, 'servo on failed.'
00752                 raise
00753         except:
00754             print self.configurator_name, 'exception occured'
00755 
00756         try:
00757             angles = self.flat2Groups(map(numpy.rad2deg,
00758                                           self.getActualState().angle))
00759             print 'Move to Actual State, Just a minute.'
00760             for i in range(len(self.Groups)):
00761                 self.setJointAnglesOfGroup(self.Groups[i][0], angles[i], tm,
00762                                            wait=False)
00763             for i in range(len(self.Groups)):
00764                 self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
00765         except:
00766             print self.configurator_name, 'post servo on motion trouble'
00767 
00768         # turn on hand motors
00769         print 'Turn on Hand Servo'
00770         if self.sc_svc:
00771             is_servoon = self.sc_svc.servoOn()
00772             print('Hands Servo on: ' + str(is_servoon))
00773             if not is_servoon:
00774                 print('One or more hand servos failed to turn on. Make sure all hand modules are properly cabled ('
00775                       + _MSG_RESTART_QNX + ') and run the command again.')
00776                 return -1
00777         else:
00778             print('hrpsys ServoController not found. Ignore this if you' +
00779                   ' do not intend to use hand servo (e.g. NEXTAGE Open).' +
00780                   ' If you do intend, then' + _MSG_RESTART_QNX +
00781                   ' and run the command again.')
00782 
00783         return 1
00784 
00785     def servoOff(self, jname='all', wait=True):
00786         '''
00787         @type jname: str
00788         @param jname: The value 'all' works iteratively for all servos.
00789         @type wait: bool
00790         @rtype: int
00791         @return: 1 = all arm servo off. 2 = all servo on arms and hands off.
00792                 -1 = Something wrong happened.
00793         '''
00794         # do nothing for simulation
00795         if self.simulation_mode:
00796             print self.configurator_name, 'omit servo off'
00797             return 0
00798 
00799         print 'Turn off Hand Servo'
00800         if self.sc_svc:
00801             self.sc_svc.servoOff()
00802         # if the servos aren't on switch power off
00803         if not self.isServoOn(jname):
00804             if jname.lower() == 'all':
00805                 self.rh_svc.power('all', SWITCH_OFF)
00806             return 1
00807 
00808         # if jname is not set properly set to all -> is this safe?
00809         if jname == '':
00810             jname = 'all'
00811 
00812         if wait:
00813             waitInputConfirm(
00814                 '!! Robot Motion Warning (Servo OFF)!!\n\n'
00815                 'Push [OK] to servo OFF (%s).' % (jname))  # :
00816 
00817         try:
00818             self.rh_svc.servo('all', SWITCH_OFF)
00819             time.sleep(0.2)
00820             if jname == 'all':
00821                 self.rh_svc.power('all', SWITCH_OFF)
00822 
00823             # turn off hand motors
00824             print 'Turn off Hand Servo'
00825             if self.sc_svc:
00826                 self.sc_svc.servoOff()
00827 
00828             return 2
00829         except:
00830             print self.configurator_name, 'servo off: communication error'
00831             return -1
00832 
00833     def checkEncoders(self, jname='all', option=''):
00834         '''
00835         Run the encoder checking sequence for specified joints,
00836         run goActual to adjust the direction values, and then turn servos on.
00837 
00838         @type jname: str
00839         @param jname: The value 'all' works iteratively for all servos.
00840         @type option: str
00841         @param option: Possible values are follows (w/o double quote):\
00842                         "-overwrite": Overwrite calibration value.
00843         '''
00844         if self.isServoOn():
00845             waitInputConfirm('Servo must be off for calibration')
00846             return
00847         # do not check encoders twice
00848         elif self.isCalibDone():
00849             waitInputConfirm('System has been calibrated')
00850             return
00851 
00852         self.rh_svc.power('all', SWITCH_ON)
00853         msg = '!! Robot Motion Warning !!\n'\
00854               'Turn Relay ON.\n'\
00855               'Then Push [OK] to '
00856         if option == '-overwrite':
00857             msg = msg + 'calibrate(OVERWRITE MODE) '
00858         else:
00859             msg = msg + 'check '
00860 
00861         if jname == 'all':
00862             msg = msg + 'the Encoders of all.'
00863         else:
00864             msg = msg + 'the Encoder of the Joint "' + jname + '".'
00865 
00866         try:
00867             waitInputConfirm(msg)
00868         except:
00869             print "If you're connecting to the robot from remote, " + \
00870                   "make sure tunnel X (eg. -X option with ssh)."
00871             self.rh_svc.power('all', SWITCH_OFF)
00872             return 0
00873 
00874         is_result_hw = True
00875         print self.configurator_name, 'calib-joint ' + jname + ' ' + option
00876         self.rh_svc.initializeJointAngle(jname, option)
00877         print self.configurator_name, 'done'
00878         is_result_hw = is_result_hw and self.rh_svc.power('all', SWITCH_OFF)
00879         self.goActual()  # This needs to happen before turning servo on.
00880         time.sleep(0.1)
00881         is_result_hw = is_result_hw and self.rh_svc.servo(jname, SWITCH_ON)
00882         if not is_result_hw:
00883             # The step described in the following msg is confirmed by the manufacturer 12/14/2015
00884             print("Turning servos ({}) failed. This is likely because of issues happening in lower level. Please check if the Kawada's proprietary tool NextageOpenSupervisor returns without issue or not. If the issue persists, contact the manufacturer.".format(jname))
00885         # turn on hand motors
00886         print 'Turn on Hand Servo'
00887         if self.sc_svc:
00888             self.sc_svc.servoOn()
00889 
00890     def startImpedance_315_1(self, arm,
00891                        M_p = 100.0,
00892                        D_p = 100.0,
00893                        K_p = 100.0,
00894                        M_r = 100.0,
00895                        D_r = 2000.0,
00896                        K_r = 2000.0,
00897                        ref_force = [0, 0, 0],
00898                        force_gain = [1, 1, 1],
00899                        ref_moment = [0, 0, 0],
00900                        moment_gain = [0, 0, 0],
00901                        sr_gain = 1.0,
00902                        avoid_gain = 0.0,
00903                        reference_gain = 0.0,
00904                        manipulability_limit = 0.1):
00905         '''
00906         @type arm: str name of artm to be controlled, this must be initialized
00907                    using setSelfGroups()
00908         @param ref_{force, moment}: Target values at the target position.
00909                                     Units: N, Nm, respectively.
00910         @param {force, moment}_gain: multipliers to the eef offset position
00911                                      vel_p and orientation vel_r. 3-dimensional
00912                                      vector (then converted internally into a
00913                                      diagonal matrix).
00914         '''
00915         ic_sensor_name = 'rhsensor'
00916         ic_target_name = 'RARM_JOINT5'
00917         if arm == 'rarm':
00918             ic_sensor_name = 'rhsensor'
00919             ic_target_name = 'RARM_JOINT5'
00920         elif arm == 'larm':
00921             ic_sensor_name = 'lhsensor'
00922             ic_target_name = 'LARM_JOINT5'
00923         else:
00924             print 'startImpedance: argument must be rarm or larm.'
00925             return
00926 
00927         self.ic_svc.setImpedanceControllerParam(
00928             OpenHRP.ImpedanceControllerService.impedanceParam(
00929                 name = ic_sensor_name,
00930                 base_name = 'CHEST_JOINT0',
00931                 target_name = ic_target_name,
00932                 M_p = M_p,
00933                 D_p = D_p,
00934                 K_p = K_p,
00935                 M_r = M_r,
00936                 D_r = D_r,
00937                 K_r = K_r,
00938                 ref_force = ref_force,
00939                 force_gain = force_gain,
00940                 ref_moment = ref_moment,
00941                 moment_gain = moment_gain,
00942                 sr_gain = sr_gain,
00943                 avoid_gain = avoid_gain,
00944                 reference_gain = reference_gain,
00945                 manipulability_limit = manipulability_limit))
00946 
00947     def stopImpedance_315_1(self, arm):
00948         ic_sensor_name = 'rhsensor'
00949         if arm == 'rarm':
00950             ic_sensor_name = 'rhsensor'
00951         elif arm == 'larm':
00952             ic_sensor_name = 'lhsensor'
00953         else:
00954             print 'startImpedance: argument must be rarm or larm.'
00955             return
00956         self.ic_svc.deleteImpedanceControllerAndWait(ic_sensor_name)
00957 
00958     def startImpedance_315_2(self, arm,
00959                        M_p = 100.0,
00960                        D_p = 100.0,
00961                        K_p = 100.0,
00962                        M_r = 100.0,
00963                        D_r = 2000.0,
00964                        K_r = 2000.0,
00965                        force_gain = [1, 1, 1],
00966                        moment_gain = [0, 0, 0],
00967                        sr_gain = 1.0,
00968                        avoid_gain = 0.0,
00969                        reference_gain = 0.0,
00970                        manipulability_limit = 0.1):
00971         '''
00972         @type arm: str name of artm to be controlled, this must be initialized
00973                    using setSelfGroups()
00974         @param {force, moment}_gain: multipliers to the eef offset position
00975                                      vel_p and orientation vel_r. 3-dimensional
00976                                      vector (then converted internally into a
00977                                      diagonal matrix).
00978         '''
00979         self.ic_svc.setImpedanceControllerParam(
00980             arm,
00981             OpenHRP.ImpedanceControllerService.impedanceParam(
00982                 M_p = M_p,
00983                 D_p = D_p,
00984                 K_p = K_p,
00985                 M_r = M_r,
00986                 D_r = D_r,
00987                 K_r = K_r,
00988                 force_gain = force_gain,
00989                 moment_gain = moment_gain,
00990                 sr_gain = sr_gain,
00991                 avoid_gain = avoid_gain,
00992                 reference_gain = reference_gain,
00993                 manipulability_limit = manipulability_limit))
00994         return self.ic_svc.startImpedanceController(arm)
00995 
00996     def startImpedance_315_3(self, arm,
00997                        M_p = 100.0,
00998                        D_p = 100.0,
00999                        K_p = 100.0,
01000                        M_r = 100.0,
01001                        D_r = 2000.0,
01002                        K_r = 2000.0,
01003                        force_gain = [1, 1, 1],
01004                        moment_gain = [0, 0, 0],
01005                        sr_gain = 1.0,
01006                        avoid_gain = 0.0,
01007                        reference_gain = 0.0,
01008                        manipulability_limit = 0.1):
01009         '''
01010         @type arm: str name of artm to be controlled, this must be initialized
01011                    using setSelfGroups()
01012         @param {force, moment}_gain: multipliers to the eef offset position
01013                                      vel_p and orientation vel_r. 3-dimensional
01014                                      vector (then converted internally into a
01015                                      diagonal matrix).
01016         '''
01017         r, p = self.ic_svc.getImpedanceControllerParam(arm)
01018         if not r:
01019             print('{}, impedance parameter not found for {}.'.format(self.configurator_name, arm))
01020             return False
01021         if M_p != None: p.M_p = M_p
01022         if D_p != None: p.M_p = D_p
01023         if K_p != None: p.M_p = K_p
01024         if M_r != None: p.M_r = M_r
01025         if D_r != None: p.M_r = D_r
01026         if K_r != None: p.M_r = K_r
01027         if force_gain != None: p.force_gain = force_gain
01028         if moment_gain != None: p.moment_gain = moment_gain
01029         if sr_gain != None: p.sr_gain = sr_gain
01030         if avoid_gain != None: p.avoid_gain = avoid_gain
01031         if reference_gain != None: p.reference_gain = reference_gain
01032         if manipulability_limit != None: p.manipulability_limit = manipulability_limit
01033         self.ic_svc.setImpedanceControllerParam(arm, p)
01034         return self.ic_svc.startImpedanceController(arm)
01035 
01036     def stopImpedance_315_2(self, arm):
01037         return self.ic_svc.stopImpedanceController(arm)
01038 
01039     def stopImpedance_315_3(self, arm):
01040         return self.ic_svc.stopImpedanceController(arm)
01041 
01042     def startImpedance(self, arm, **kwargs):
01043         '''
01044         Enable the ImpedanceController RT component.
01045         This method internally calls startImpedance-*, hrpsys version-specific
01046         method.
01047 
01048         @requires: ImpedanceController RTC to be activated on the robot's
01049                    controller.
01050         @param arm: Name of the kinematic group (i.e. self.Groups[n][0]).
01051         @param kwargs: This varies depending on the version of hrpsys your
01052                        robot's controller runs on
01053                        (which you can find by "self.hrpsys_version" command).
01054                        For instance, if your hrpsys is 315.10.1, refer to
01055                        "startImpedance_315_4" method.
01056         @change: (NOTE: This "change" block is a duplicate with the PR in the
01057                  upstream https://github.com/fkanehiro/hrpsys-base/pull/1120.
01058                  Once it gets merged this block should be removed to avoid
01059                  duplicate maintenance effort.)
01060 
01061                  From 315.2.0 onward, following arguments are dropped and can
01062                  be set by self.seq_svc.setWrenches instead of this method.
01063                  See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44:
01064                  - ref_force[fx, fy, fz] (unit: N) and
01065                    ref_moment[tx, ty, tz] (unit: Nm) can be set via
01066                    self.seq_svc.setWrenches. For example:
01067 
01068                    self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0,
01069                                              fx, fy, fz, tx, ty, tz,
01070                                              0, 0, 0, 0, 0, 0,
01071                                              0, 0, 0, 0, 0, 0,])
01072 
01073                    setWrenches takes 6 values per sensor, so the robot in
01074                    the example above has 4 sensors where each line represents
01075                    a sensor. See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example.
01076         '''
01077         if StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
01078             self.startImpedance_315_1(arm, **kwargs)
01079         elif StrictVersion(self.hrpsys_version) < StrictVersion('315.3.0'):
01080             self.startImpedance_315_2(arm, **kwargs)
01081         else:
01082             self.startImpedance_315_3(arm, **kwargs)
01083         print('startImpedance {}'.format(self._MSG_IMPEDANCE_CALL_DONE))
01084 
01085     def stopImpedance(self, arm):
01086         if StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
01087             self.stopImpedance_315_1(arm)
01088         elif StrictVersion(self.hrpsys_version) < StrictVersion('315.3.0'):
01089             self.stopImpedance_315_2(arm)
01090         else:
01091             self.stopImpedance_315_3(arm)
01092         print('stopImpedance {}'.format(self._MSG_IMPEDANCE_CALL_DONE))
01093 
01094     def removeForceSensorOffset(self):
01095         self.rh_svc.removeForceSensorOffset()
01096 
01097 
01098     # workaround for issue reported https://github.com/tork-a/rtmros_nextage/issues/332
01099     def getJointAnglesOfGroup(self, limb):
01100         angles = self.getJointAngles()
01101         # fix flag2groups for hironx(sim), which have gripper joints
01102         # flat2Groups:  requres 315.2.0
01103         # angles = self.flat2Groups(angles)
01104         offset = 0
01105         if len(angles) != reduce(lambda x,y: x+len(y[1]), self.Groups, 0):
01106             offset = 4
01107         angles = []
01108         index = 0
01109         for group in self.Groups:
01110             joint_num = len(group[1])
01111             angles.append(angles[index: index + joint_num])
01112             index += joint_num
01113             if group[0] in ['larm', 'rarm']:
01114                 index += offset ## FIX ME
01115         groups = self.Groups
01116         for i in range(len(groups)):
01117             if groups[i][0] == limb:
01118                 return angles[i]
01119         print self.configurator_name, 'could not find limb name ' + limb
01120         print self.configurator_name, ' in' + filter(lambda x: x[0], groups)
01121 
01122     def clearOfGroup(self, limb):
01123         '''!@brief
01124         Clears the Sequencer's current operation for joint groups.
01125         @since 315.5.0
01126         '''
01127         if StrictVersion(self.seq_version) < StrictVersion('315.5.0'):
01128             raise RuntimeError('clearOfGroup is not available with your '
01129                                'software version ' + self.seq_version)
01130         HrpsysConfigurator.clearOfGroup(self, limb)
01131         angles = self.getJointAnglesOfGroup(limb)
01132         print self.configurator_name, 'clearOfGroup(' + limb + ') will send ' + str(angles) + ' to update seqplay until https://github.com/fkanehiro/hrpsys-base/pull/1141 is available'
01133         self.setJointAnglesOfGroup(limb, angles, 0.1, wait=True)


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37