nextageをOpenRTM-aistで使う

文書の過去の版を表示しています。

カワダロボティクス株式会社の双椀ロボットnexgateは，カワダ純正の作業プログラミング環境が提供されている普通のnextageと一般社団法人東京オープンソースロボティクス協会（tork）が提供するros/rtm¹⁾でのプログラミング環境の利用が推奨されている研究用プラットフォームnextage-openとが存在している．

従来，カワダではhironxというほぼ同スペック²⁾のものをrtmベースで提供していた．ユーザインタフェースはJython上のプログラム(hrpsys.pyおよびrtm.py) ³⁾ で提供されていた．

電通大の私が所属していた研究室では，この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に関するまとまった情報がないので，ソースコードおよび利用手順を追いかけて解析していく必要がある．

チュートリアルではまず最初に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 ⁴⁾ のリストを返す．すなわち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と同じなのでそれを使うことにする．

init関数

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）が実行されているかどうかをチェックして

起動されていれば，findComps
そうでなければ，必要なrtcの生成，接続，活性化，ログの設定（createComps connectComps, activateComps, setupLogger）を行う

        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)

    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

OffPoseにして，servoOff．動作方法の参考になる．

import rtm
 
from rtm import *
from OpenHRP import *
from hrpsys import *  # load ModelLoader
from hrpsys import ImpedanceControllerService_idl
 
from waitInput import waitInputConfirm

ここでは主にHrpsysCofgiguratorを定義している．ヒューマノイドロボットを基本とした

汎用的なロボット用のrtcの情報の管理，

それに基づく

コンポーネントの生成／削除，活性／非活性化，接続，検索，
種々のコンポーネントのリスト化，
身体情報，センサ情報，
ロガーの設定，接続，
マネージャ，ハードウェアrtc，ModelLoaderの処理

および

サーボオン／オフ
種々の動作制御

を行っている．

明示的な初期化処理として以下を行っている．ただしこの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

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 のとき動作命令が呼ばれないように見えるのは気のせいだろうか？

bodyinfo

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を行う．

rtcのロード/生成

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の接続

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])

¹⁾

ここのrtmはOpenRTM-aistのこと

²⁾

新しいものはいろいろ改良が施されている

³⁾

このhrpsysとrtmはおそらく現在torkが提供しているものの前身からの分岐で至るところに変更がある．

⁴⁾

RobotHardwareあるいはシミュレータのロボットを除く

nextageをOpenRTM-aistで使う

rosのrtm-pythonインタフェース

nextage.py

nextage_client

hironx_client

init関数

hrpsys_config

nextage_handleに必要な情報

Groups

goOffPose

goInitial

hiro_handle.pyの書き換え

bodyinfo

OpenHRP

init0

init1

rtcのロード/生成

rtcの接続

RoboLab.Suehiro