カワダロボティクス株式会社の双椀ロボットnexgateは， カワダ純正の作業プログラミング環境が提供されている普通のnextageと 一般社団法人東京オープンソースロボティクス協会（tork）が 提供するros/rtm1)でのプログラミング環境の利用が推奨されている 研究用プラットフォームnextage-openとが存在している．
従来，カワダではhironxというほぼ同スペック2)のものをrtmベースで 提供していた．ユーザインタフェースはJython上のプログラム(hrpsys.pyおよびrtm.py) 3) で提供されていた．
電通大の私が所属していた研究室では，このhironxのシステムを 通常のpythonで扱えるようにrtc_handle.pyをベースとしたhiro_handle.pyを用いて直接操作することで様々な動作／作業を実現している．
最近，nextage-openを用いて同様な作業を実現しようとしたところ， torkが提供するシステムのrtm部分は従来のカワダのものとかなり異なるものとなっていたので それへの対応についてのメモを残すことにした．
rtmros_nextageのnextage_ros_bridgeにはipythonでrtmを直接操作するためのインタフェースおよび そのチュートリアルが用意されている．
問題はその環境構築にrosを使わなくてはならないということである．
このシステムはgazeboを利用したシミュレータを提供しており，実機がなくてもロボットを動作させるためのテストも容易に行えるので非常に便利で優れたものである．
しかし， rosを勉強する必要があるだけでなく，rosはosやros自身のバージョンの整合を取る必要があり 最新のosやその環境を利用しようとしたときに大きな足かせとなる． そのため実機があり，それをrtmだけで使う場合はrosが邪魔になる．
一方，rtmは個々のマシンのosやrtmのバージョンが異なっていても問題なく連携させることができるという自由さがある．
いずれにしてもnextage_ros_brigeに関するまとまった情報がないので，ソースコードおよび利用手順を追いかけて 解析していく必要がある．
基本的な継承関係は以下のようになっている4)．
クラスとしてはHrpsysCofgiguratorがスーパークラスでその下がサブクラスである．
問題は，HrpsysCofgiguratorがヒューマノイドロボットを含む多数のロボットの機能（メソッド）をいろいろ含んでいて， サブクラスではそのサブセットの機能を使う構造になっているという点である．
サブクラスなのだから機能もそのサブセットで良いと思ったとしたら大間違い．オブジェクト指向を理解していないと言える． 5)
そのためプログラムを追うことがとても難しくなっている．下から順に上に追っていくに従って， どれを読まなくても良いのかよくわからない状態で読まなくても良い他のロボット用のプログラムを読まされることになる．
とても困ったものである．
チュートリアルではまず最初にnextage.pyを読み込んでいる． これはrtmros_nextage-indigo-devel/nextage_ros_bridge/script/nextage.pyである．
そこでは，nextage_clientなどのモジュールを読み込んでいる． 他にros関係も読み込んでいる．
from hironx_ros_bridge.ros_client import ROS_Client from nextage_ros_bridge import nextage_client
ただnextage_client.pyではhironx_client.pyインポートしており，多くの処理がhironx_clientに投げられている． またROS_clientもhironx_ros_bridge.ros_clientのものを使っているので先にそれを読み込む必要がある．
ちなみにhironx.pyの場合は以下となる．
from hironx_ros_bridge import hironx_client from hironx_ros_bridge.ros_client import ROS_Client
またrtm関連としてhrpsysからrtmモジュールを読み込んでいる．
from hrpsys import rtm
以下はガードをつけての処理で，これをトップで読み込んだときの環境構築を実行している．
引数を処理して，rtm関連のパラメタをセット
if __name__ == '__main__': args, unknown = parser.parse_known_args() if args.host: rtm.nshost = args.host if args.port: rtm.nsport = args.port if not args.robot: args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0" if not args.modelfile: args.modelfile = "/opt/jsk/etc/NEXTAGE/model/main.wrl" if args.host else ""
rtm.nshostは一般にはcosネームサーバが走っているホストだがここではqnxが走っているロボットコントローラで， rtm.nsportはそのポート番号．
args.robotはロボット本体のrtc名．args.hostの指定がない場合はそこでgazeboシミュレータが走っているとしている．
args.modelfileは，nextageのvrmlモデルのurl.シミュレータでは不要なのだろう．
その後，以下を実行している（面倒なのでtry, except部は省く）．
robot = nxc = nextage_client.NextageClient() robot.init(robotname=args.robot, url=args.modelfile) if args.dio_ver: robot.set_hand_version(args.dio_ver) ros = ROS_Client()
robot.initでrtm関連を立ち上げを行っている．これはnextage_client.pyを見る必要がある．
args.dio_verは，torkサポートのハンドで使う．他のハンドを使うときは不要．
rosの部分は今回は使用しないのでとりあえず無視する．
nextage_client.pyは，nextage_ros_bridge/src/nextage_ros_bridgeにある．
ここでは最初にhironx_clientからHIRONXを読み込んでいる．
from hironx_ros_bridge.hironx_client import HIRONX
他にもros関連やhand関連のモジュールを読み込んでいる． とりあえず関係ありそうなものだけを調べる．
nextage_client.pyではNextageClientクラスの定義のみで，それはHIRONXのサブクラスとなっている．
NextageClientでは，HIRONXのアームのパラメタやいくつかの関数を上書きしている．
基本的なプログラム群の構造は，何でもできるsuperclassの機能を 部分的に利用する形でsubclass側で上書きしている． オブジェクト指向のプログラミングスタイルからすると非常に読みにくい少し邪道な構造になっている．
最初の方で，停止姿勢を定義している．これはhironex_clientを上書きしたものとなっている．
OffPose = [[0], [0, 0], [25, -140, -150, 45, 0, 0], [-25, -140, -150, -45, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]
この構造は後述のHIRONXのGroupsで定義され，その構造はSequencePlayer(seq)RTCへ設定することで データの部分書き換えなどの整合性をとっている．
__init__関数ではgazeboシミュレータの利用をチェックしている．gazeboを使わない場合はHIRONXの__init__を呼び出す．
gazeboを使う方の処理は今回は無視する．
ほとんどの処理はinit関数で行われる．init関数は前述のnextage.pyのガード付き実行部で呼ばれる． ただしNextageClientのinitは，以下のようにHIRONXのinitを呼び出すだけである．
後述するがコメントにあるようにHIRONX（およびそのsuperclassであるHrpsysConfigurator）のinitでは， 必要なrtcの生成，接続や起動およびseqへのGroupsの設定を行う．
def init(self, robotname="HiroNX(Robot)0", url=""): ''' Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. Also runs config, logger. Also internally calls setSelfGroups(). @type robotname: str @type url: str ''' if not self.use_gazebo: HIRONX.init(self, robotname=robotname, url=url)
以下の関数ではnextageで使うrtc 6) のリストを返す．すなわちnextageで使うrtcは，seq,sh,fk,el,ic,logということになる．
def getRTCList(self): ''' Overwriting HrpsysConfigurator.getRTCList Returning predefined list of RT components. @rtype [[str]] @return List of available components. Each element consists of a list of abbreviated and full names of the component. ''' return [ ['seq', "SequencePlayer"], ['sh', "StateHolder"], ['fk', "ForwardKinematics"], ['el', "SoftErrorLimiter"], # ['co', "CollisionDetector"], # ['sc', "ServoController"], ['ic', "ImpedanceController"], ['log', "DataLogger"] ]
また以下はInitPoseと呼ばれる動作初期位置への移動命令である．init_pos_typeは工場出荷設定とユーザ設定などを振り分ける．
def goInitial(self, tm=7, wait=True, init_pose_type=0): ''' @see: HIRONX.goInitial ''' if not init_pose_type: # Set the pose where eefs level with the tabletop by default. init_pose_type = HIRONX.INITPOS_TYPE_EVEN return HIRONX.goInitial(self, tm, wait, init_pose_type)
その他，handおよびdio関係の関数が多数定義されている． handに関してはtorkが提供しているものを使わないので無視する．
ここに書かれていない関数に関しては親クラスを順に探すことになる．
hironx_client.pyは，hironx_ros_bridge/src/hironx_ros_bridgeにある．
まずはここでの初期パラメタ設定とinit(= HIRONX.init)の振る舞いを見る必要がある． rtm関連モジュールとして以下が読み込まれている．
from hrpsys.hrpsys_config import * # hot fix for https://github.com/start-jsk/rtmros_hironx/issues/539 # On 16.04 (omniorb4-dev 4.1) if we start openhrp-model-loader with # <env name="ORBgiopMaxMsgSize" value="2147483648" /> # all connection(?) conect respenct giopMaxMsgSize # But on 18.04 (omniorb4-dev 4.2) wnneed to set ORBgiopMaxMsgSize=2147483648 # for each clients if not os.environ.has_key('ORBgiopMaxMsgSize'): os.environ['ORBgiopMaxMsgSize'] = '2147483648' import OpenHRP import OpenRTM_aist import OpenRTM_aist.RTM_IDL import rtm
まずHrpsysConfigurator2クラスをHrpsysConfiguratorのサブクラスとして定義している．
ここでは主に，関節パラメタの設定，取得関するヘルパーをHIRONX用に定義している． 本来ならHIRONXにちゃんと定義すれば良いのだが，test用とコメントされている． 定義されているのは以下の4つ．
def _get_geometry(self, method, frame_name=None): def getCurrentPose(self, lname=None, frame_name=None): def getReferencePose(self, lname=None, frame_name=None): def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
HIRONXクラスはHrpsysConfigurator2のサブクラスとして定義されている．
まず，関節構造のグループを定義している．これはnextageでも使っている．
Groups = [['torso', ['CHEST_JOINT0']], ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']], ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']], ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']]]
OffPoseに関しては，nextageとは微妙に異なるものが設定されている． 構造定義は少し異なるが，その値はhiro_handle.pyのものと同じである．
OffPose = [[0], [0, 0], [25, -139, -157, 45, 0, 0], [-25, -139, -157, -45, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]
しかしnextageにはこれは使わず，当然nextageのものを使うことになる．
InitialPoseはユーザ定義と工場出荷設定の2つが用意されている．
# With this pose the EEFs level up the tabletop surface. _InitialPose = [[0], [0, 0], [-0.6, 0, -100, 15.2, 9.4, 3.2], [0.6, 0, -100, -15.2, 9.4, -3.2], [0, 0, 0, 0], [0, 0, 0, 0]] # This pose sets joint angles at the factory initial pose. No danger, but # no real advange either for in normal usage. # See https://github.com/start-jsk/rtmros_hironx/issues/107 _InitialPose_Factory = [[0], [0, 0], [-0, 0, -130, 0, 0, 0], [0, 0, -130, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]] INITPOS_TYPE_EVEN = 0 INITPOS_TYPE_FACTORY = 1
ユーザ定義の方はhiro_handle.pyと同じなのでそれを使うことにする．
HIRONXクラスでは,hironx用にinit，その他の関数を上書きしている．
initは，（これはむしろhrpsys.pyに近い処理を行っている．）
def init(self, robotname="HiroNX(Robot)0", url=""): ''' Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. Also runs config, logger. Also internally calls setSelfGroups(). @type robotname: str @type url: str ''' # reload for hrpsys 315.1.8 print(self.configurator_name + "waiting ModelLoader") HrpsysConfigurator.waitForModelLoader(self) print(self.configurator_name + "start hrpsys")
ModelLoderでvrmlモデルのロードを行っている．この部分はシミュレータモデルの構築だけでなく，アームの幾何構造の取得に必要となる．
print(self.configurator_name + "finding RTCManager and RobotHardware") HrpsysConfigurator.waitForRTCManagerAndRoboHardware(self, robotname=robotname) print self.configurator_name, "Hrpsys controller version info: " if self.ms : print self.configurator_name, " ms = ", self.ms if self.ms and self.ms.ref : print self.configurator_name, " ref = ", self.ms.ref if self.ms and self.ms.ref and len(self.ms.ref.get_component_profiles()) > 0: print self.configurator_name, " version = ", self.ms.ref.get_component_profiles()[0].version 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'): sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hrpsys_315_1_9/hrpsys')) delete_module('ImpedanceControllerService_idl') import AbsoluteForceSensorService_idl import ImpedanceControllerService_idl
qnxで事前に走っていなければならないRTCManagerとRobotHardwareの起動をチェックして，必要な情報を取得している．
# HrpsysConfigurator.init(self, robotname=robotname, url=url) self.sensors = self.getSensors(url)
標準ではnextage-openにはセンサがないのでこれは無視する．
このようにnextage-open非標準の機能をサポートするコードがtork関係者が参加したプロジェクト用にいろいろ定義されていてコードが読みづらい．
これはhandに関しても同様．
# all([rtm.findRTC(rn[0], rtm.rootnc) for rn in self.getRTCList()]) # not working somehow... if set([rn[0] for rn in self.getRTCList()]).issubset(set([x.name() for x in self.ms.get_components()])) : print(self.configurator_name + "hrpsys components are already created and running") self.findComps(max_timeout_count=0, verbose=True) else: print(self.configurator_name + "no hrpsys components found running. creating now") self.createComps() print(self.configurator_name + "connecting hrpsys components") self.connectComps() print(self.configurator_name + "activating hrpsys components") self.activateComps() print(self.configurator_name + "setup hrpsys logger") self.setupLogger()
必要とされているrtc（getRTCList）が実行されているかどうかをチェックして
print(self.configurator_name + "setup joint groups for hrpsys controller") self.setSelfGroups() print(self.configurator_name + '\033[32minitialized successfully\033[0m') # set hrpsys_version try: self.hrpsys_version = self.fk.ref.get_component_profile().version except: print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m') pass self.setSelfGroups() self.hrpsys_version = self.fk.ref.get_component_profile().version
seqにGroupsの情報を設定している．なぜ2回繰り返しているのかは不明．
def goOffPose(self, tm=7): ''' Move arms to the predefined (as member variable) pose where robot can be safely turned off. @type tm: float @param tm: Second to complete. ''' for i in range(len(self.Groups)): self.setJointAnglesOfGroup(self.Groups[i][0], self.OffPose[i], tm, wait=False) for i in range(len(self.Groups)): self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0]) self.servoOff(wait=False)
OffPoseにして，servoOff． 動作方法の参考になる．
def goInitial(self, tm=7, wait=True, init_pose_type=0): ''' Move arms to the predefined (as member variable) "initialized" pose. @type tm: float @param tm: Second to complete. @type wait: bool @param wait: If true, other command to the robot's joints wait until this command returns (done by running SequencePlayer.waitInterpolationOfGroup). @type init_pose_type: int @param init_pose_type: 0: default init pose (specified as _InitialPose) 1: factory init pose (specified as _InitialPose_Factory) ''' if init_pose_type == self.INITPOS_TYPE_FACTORY: _INITPOSE = self._InitialPose_Factory else: _INITPOSE = self._InitialPose ret = True for i in range(len(self.Groups)): # radangles = [x/180.0*math.pi for x in self._InitialPose[i]] print( '{}, JntAnglesOfGr={}, INITPOSE[i]={}, tm={}, wait={}'.format( self.configurator_name, self.Groups[i][0], _INITPOSE[i], tm, wait)) ret &= self.setJointAnglesOfGroup(self.Groups[i][0], _INITPOSE[i], tm, wait=False) if wait: for i in range(len(self.Groups)): self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0]) return ret
nextage_clientのgoInitialから工場出荷設定でないこと（ユーザ設定）を指定して動作させている．
nextage_clientで完全に上書きされている．
seqにGroups情報を設定する．HIRONX.intから呼ばれている．
def setSelfGroups(self): ''' Set to the hrpsys.SequencePlayer the groups of links and joints that are statically defined as member variables (Groups) within this class. That said, override Groups variable if you prefer link and joint groups set differently. ''' #TODO: Accept groups variable. The name of the method sounds more # natural if it accepts it. for item in self.Groups: self.seq_svc.addJointGroup(item[0], item[1]) for k, v in self.HandGroups.iteritems(): if self.sc_svc: self.sc_svc.addJointGroup(k, v)
servoOnから呼ばれる．
def isCalibDone(self): ''' Check whether joints have been calibrated. @rtype bool ''' if self.simulation_mode: return True else: rstt = self.rh_svc.getStatus() for item in rstt.servoState: if not item[0] & 1: return False return True
HIRONX.servoOnから呼ばれる．
def isServoOn(self, jname='any'): ''' Check whether servo control has been turned on. Check is done by HIRONX.getActualState().servoState. @type jname: str @param jname: Name of a link (that can be obtained by "hiro.Groups" as lists of groups). Reserved values: - any: This command will check all servos available. - all: Same as 'any'. @rtype bool @return: If jname is specified either 'any' or 'all', return False if the control of any of servos isn't available. ''' if self.simulation_mode: return True else: s_s = self.getActualState().servoState if jname.lower() == 'any' or jname.lower() == 'all': for s in s_s: # print self.configurator_name, 's = ', s if (s[0] & 2) == 0: return False else: jid = eval('self.' + jname) print self.configurator_name, s_s[jid] if s_s[jid][0] & 1 == 0: return False return True return False
いろいろとチェックしている． 現在位置と目標位置の整合性などもとっているようだ．
必要な部分だけ抜き出すためにはもう少し解析する必要がある．
rh_svc.power, rh_svc.servoOnについても調べる必要がある．
いろいろとチェックしている． 現在位置と目標位置の整合性などもとっているようだ．
必要な部分だけ抜き出すためにはもう少し解析する必要がある．
rh_svc.power, rh_svc.servoOffについても調べる必要がある．
要解析．
要解析
Groupsの設定の解除？
要解析
import rtm from rtm import * from OpenHRP import * from hrpsys import * # load ModelLoader from hrpsys import ImpedanceControllerService_idl from waitInput import waitInputConfirm
ここでは主にHrpsysCofgiguratorを定義している． ヒューマノイドロボットを基本とした
それに基づく
および
を行っている．
サブクラスから呼び出されているものおよびさらにそこから呼び出されているものをピックアップする．
上書きされているもの
明示的な初期化処理として以下を行っている．ただしこのinitはHIRONXで上書きされているため呼び出されることはない． 参考のためコードを引用する．
def init(self, robotname="Robot", url=""): '''!@brief Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. Also runs config, logger. Also internally calls setSelfGroups(). @type robotname: str @type url: str ''' print(self.configurator_name + "waiting ModelLoader") self.waitForModelLoader() print(self.configurator_name + "start hrpsys") print(self.configurator_name + "finding RTCManager and RobotHardware") self.waitForRTCManagerAndRobotHardware(robotname) self.sensors = self.getSensors(url) print(self.configurator_name + "creating components") self.createComps() print(self.configurator_name + "connecting components") self.connectComps() print(self.configurator_name + "activating components") self.activateComps() print(self.configurator_name + "setup logger") self.setupLogger() print(self.configurator_name + "setup joint groups") self.setSelfGroups() print(self.configurator_name + '\033[32minitialized successfully\033[0m') # set hrpsys_version try: self.hrpsys_version = self.fk.ref.get_component_profile().version except: print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m') pass def __init__(self, cname="[hrpsys.py] "): initCORBA() self.configurator_name = cname
def waitForRTCManager(self, managerhost=nshost): '''!@brief Wait for RTC manager. @param managerhost str: name of host computer that manager is running ''' self.ms = None while self.ms == None: time.sleep(1) if managerhost == "localhost": managerhost = socket.gethostname() self.ms = rtm.findRTCmanager(managerhost) print(self.configurator_name + "wait for RTCmanager : " + str(managerhost))
def waitForRobotHardware(self, robotname="Robot"): '''!@brief Wait for RobotHardware is exists and activated. @param robotname str: name of RobotHardware component. ''' self.rh = None timeout_count = 0 # wait for simulator or RobotHardware setup which sometime takes a long time while self.rh == None and timeout_count < 10: # <- time out limit if timeout_count > 0: # do not sleep initial loop time.sleep(1); self.rh = rtm.findRTC("RobotHardware0") if not self.rh: self.rh = rtm.findRTC(robotname) print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count)) if self.rh and self.rh.isActive() == None: # just in case rh is not ready... self.rh = None timeout_count += 1 if not self.rh: print(self.configurator_name + "Could not find " + robotname) if self.ms: print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) print(self.configurator_name + "Exitting.... " + robotname) exit(1) print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (self.rh.name(), self.rh, self.rh.isActive()))
def getJointAngleControllerList(self): '''!@brief Get list of controller list that need to control joint angles ''' controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] return filter(lambda c: c != None, controller_list) # only return existing controllers
hrpsys_cofig では汎用性を考えてロボットの関節パラメタ(q,qRef)などをRTM::TimedDoubleSequenceでやり取りしている．
ただしこれだと常にすべての関節を考えてデータを作成しなくてはならなくなるのでロボットに応じた関節パラメタの Groupsを定義して，SequencePlayerServiceではグループごとの処理を行えるようにしてある．
hironx/nextageでは以下のグループが定義されている．SequencePlayerServiceを使った動作，たとえばgoOffPoseなどでは データもグループごとのlistで与えるようになっている．
Groups = [['torso', ['CHEST_JOINT0']], ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']], ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']], ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']]]
実際のサーボループへ送るときは，現在の指令値とマージして単一のlistで送ることになる．
SequencePlayerには最初にこのグループを伝えておく必要がある．
def setSelfGroups(self): '''!@brief Set to the hrpsys.SequencePlayer the groups of links and joints that are statically defined as member variables (Groups) within this class. ''' for item in self.Groups: self.seq_svc.addJointGroup(item[0], item[1])
hiroのbodyinfo.pyにて，
timeToOffPose = 10.0 # [sec] offPose = [ [0, 0, 0], [25, -139, -157, 45, 0, 0], [-25, -139, -157, -45, 0, 0], [], # [ 0, 0, 0, 0], [], # [ 0, 0, 0, 0], ]
これはhiro_handle.pyでは以下のメソッドで使われている．
def goOffPose(self, tm=bodyinfo.timeToOffPose, wait = True): self.setJointAnglesDeg(bodyinfo.offPose, tm, wait=wait) if wait: self.servoOff(doConfirm=False)
これはnextage_client.pyで，以下のようにhironex_clientを上書きしており，
OffPose = [[0], [0, 0], [25, -140, -150, 45, 0, 0], [-25, -140, -150, -45, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]
以下のメソッドで使われている．
def goOffPose(self, tm=7): ''' Move arms to the predefined (as member variable) pose where robot can be safely turned off. @type tm: float @param tm: Second to complete. ''' for i in range(len(self.Groups)): self.setJointAnglesOfGroup(self.Groups[i][0], self.OffPose[i], tm, wait=False) for i in range(len(self.Groups)): self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0]) self.servoOff(wait=False)
timeToOffPoseはtmとして直打ちされており7なので,nextage_handeleとしては
timeToOffPose=7
とするか，hironx_clientのように直打ちにすればよい．10が7になっているのは性能アップのおかげか？
hiroのbodyinfo.pyにて，
timeToInitialPose = 10.0 # [sec] initialPose = [ [ 0, 0, 0], [-0.6, 0, -100, 15.2, 9.4, 3.2], [ 0.6, 0, -100, -15.2, 9.4, -3.2], [], # [ 0, 0, 0, 0], [], # [ 0, 0, 0, 0], ]
これはサーボオフの必要がないのでとてもシンプル．
def goInitial(self, tm=bodyinfo.timeToInitialPose, wait = True): self.setJointAnglesDeg(bodyinfo.initialPose, tm, wait=wait)
一方，hironnx_clientでは，工場設定（キャリブレーション終了姿勢？）とユーザ設定が異なるため以下のようになっている．
# With this pose the EEFs level up the tabletop surface. _InitialPose = [[0], [0, 0], [-0.6, 0, -100, 15.2, 9.4, 3.2], [0.6, 0, -100, -15.2, 9.4, -3.2], [0, 0, 0, 0], [0, 0, 0, 0]] # This pose sets joint angles at the factory initial pose. No danger, but # no real advange either for in normal usage. # See https://github.com/start-jsk/rtmros_hironx/issues/107 _InitialPose_Factory = [[0], [0, 0], [-0, 0, -130, 0, 0, 0], [0, 0, -130, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]] INITPOS_TYPE_EVEN = 0 INITPOS_TYPE_FACTORY = 1
def goInitial(self, tm=7, wait=True, init_pose_type=0): ''' Move arms to the predefined (as member variable) "initialized" pose. @type tm: float @param tm: Second to complete. @type wait: bool @param wait: If true, other command to the robot's joints wait until this command returns (done by running SequencePlayer.waitInterpolationOfGroup). @type init_pose_type: int @param init_pose_type: 0: default init pose (specified as _InitialPose) 1: factory init pose (specified as _InitialPose_Factory) ''' if init_pose_type == self.INITPOS_TYPE_FACTORY: _INITPOSE = self._InitialPose_Factory else: _INITPOSE = self._InitialPose ret = True for i in range(len(self.Groups)): # radangles = [x/180.0*math.pi for x in self._InitialPose[i]] print( '{}, JntAnglesOfGr={}, INITPOSE[i]={}, tm={}, wait={}'.format( self.configurator_name, self.Groups[i][0], _INITPOSE[i], tm, wait)) ret &= self.setJointAnglesOfGroup(self.Groups[i][0], _INITPOSE[i], tm, wait=False) if wait: for i in range(len(self.Groups)): self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0]) return ret
コードを見る限りデフォールトはself._InitailPoseのようである．
よくわからないのは wait=False のとき動作命令が呼ばれないように見えるのは気のせいだろうか？
import bodyinfo
今の所対応ファイルは見当たらない．
ただし重要そうなのはoffPoseとinitialPoseだけののようなので問題はないと思う．
import OpenHRP from OpenHRP import RobotHardwareService as RHS SwitchStatus=RHS
基本的には，RobotHardwareService.idlのstubの読み込み．
idlの比較が必要
とりあえず必要なのはmanager, SystemAdmin, RobotHardware
def init0(self) : ns = self.name_space[self.ns_hiro] ns.list_obj() self.hiro_manager =ns.obj_list[self.ns_hiro+'.host_cxt/manager.mgr'] self.adm=ns.rtc_handles['SystemAdmin0.rtc'] self.adm_svc=self.adm.services['SystemAdminService'].provided['service0'].ref self.adm.activate() self.rh=ns.rtc_handles['RobotHardware0.rtc'] self.rh_svc=self.rh.services['RobotHardwareService'].provided['service0'].ref
managerとRobotHardwareはhrpsys_configのwaitForRTCManager(self, managerhost=nshost)と waitForRobotHardware(self, robotname=“Robot”)で行っている．
その呼び出しは，init(self, robotname=“Robot”, url=“”)で行っている．
しかし，HIRONXからはこのinitの呼び出しは行わず，自身のinitで必要なHrpsysConfiguratorのメソッドを 呼び出して，同様な処理を行っている．
hrpsys.pyはスクリプトとして実行したときのみその呼び出しを行っている．
SystemAdminServiceは見当たらない．これはshutdown, rebootに用いるのみなのでsshで代用可能．
ここではrtcのロード/生成,接続，activateを行う．
managerで必要なrtcのロード/生成を行う．問題となるのはそれが何かということ．
rtcのロード/生成はHrpsysConfiguratorのcreateCompsで行っている． 対象となるrtcのリストはHIRONX,NextageClientで上書きしたgetRTCList()で得る． NextageClientのgetRTCListは，
def getRTCList(self): ''' Overwriting HrpsysConfigurator.getRTCList Returning predefined list of RT components. @rtype [[str]] @return List of available components. Each element consists of a list of abbreviated and full names of the component. ''' return [ ['seq', "SequencePlayer"], ['sh', "StateHolder"], ['fk', "ForwardKinematics"], ['el', "SoftErrorLimiter"], # ['co', "CollisionDetector"], # ['sc', "ServoController"], ['ic', "ImpedanceController"], ['log', "DataLogger"] ]
すなわち，利用しているrtcはseq,sh,fk,el,ic,logということになる．
また，各rtcのサービスポートの取得も行っている．
rtcの接続はHrpsysConfiguratorのconnectCompsで行っている． これは汎用的に作られているので必要なところだけ抜き出す必要がある．
メインループはJointAngleControllerの接続であり，これはgetJointAngleControllerListで得る．
def getJointAngleControllerList(self): '''!@brief Get list of controller list that need to control joint angles ''' controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] return filter(lambda c: c != None, controller_list) # only return existing controllers
存在しないrtcは返さないので，[ic,el]が返る．
接続順は，
sh[qOut] -> [qRef]ic[q] -> [qRef]el[q] -> [qRef]rh となる．
rhに関しては，
rh[servoState] -> [servoStateIn]el
sh,seq,fkに関しては，
rh[q] -> [currentQIn]sh,[q]fk sh[qOut] -> [qRef]fk seq[qRef] -> [qIn]sh seq[tqRef] -> [tqIn]sh seq[basePos] -> [basePosIn]sh seq[baseRpy] -> [baseRpyIn]sh seq[zmpRef] -> [zmpIn]sh seq[optionalData] -> [optionalDataIn]sh sh[basePosOut] -> [basePosInit]seq, [basePosRef]fk sh[baseRpyOut] -> [baseRpyInit]seq, [baseRpyRef]fk sh[qOut] -> [qInit]seq sh[zmpOut] -> [zmpRefInit]seq
icに関しては，
rh[q] -> [qCurrent]ic sh[basePoseOut] -> [basePoseIn]ic sh[baseRpyOut] -> [baseRpyIn]ic
elに関しては，
rh[q] -> [qCurrent]el
logのポートへの接続はlog_svc.addでポートを生成しながら行う．よってlogを先にactivateしておく必要がある．
HrpsysConfiguratorでのsh,ic,el,rhの接続は，
if self.sh != None: self.connectLoggerPort(self.sh, 'qOut') self.connectLoggerPort(self.sh, 'tqOut') self.connectLoggerPort(self.sh, 'basePosOut') self.connectLoggerPort(self.sh, 'baseRpyOut') self.connectLoggerPort(self.sh, 'zmpOut') if self.ic != None: self.connectLoggerPort(self.ic, 'q') if self.el != None: self.connectLoggerPort(self.el, 'q') if self.rh != None: self.connectLoggerPort(self.rh, 'emergencySignal', 'emergencySignal') self.connectLoggerPort(self.rh, 'servoState') self.log_svc.clear()
接続完了後，一旦ログのクリアを行っている．
またサーボと別なexecution contextで処理を行う場合は以下を行っている．
## parallel running log process (outside from rtcd) for saving logs by emergency signal if self.log and (self.log_use_owned_ec or not isinstance(self.log.owned_ecs[0], OpenRTM._objref_ExtTrigExecutionContextService)): print(self.configurator_name + "\033[32mruning DataLogger with own Execution Context\033[0m") self.log.owned_ecs[0].start() self.log.start(self.log.owned_ecs[0])
キャリブレションの実施確認
hiro_handle:なし
nextage: