差分

このページの2つのバージョン間の差分を表示します。

--- articles:nextage_rtm [2021/08/26 11:54] – [hironx_client] Takashi Suehiro
+++ articles:nextage_rtm [2021/09/22 11:16] (現在) – [init1] Takashi Suehiro
@@ 行 36: / 行 36: @@
 いずれにしてもnextage_ros_brigeに関するまとまった情報がないので，ソースコードおよび利用手順を追いかけて
 解析していく必要がある．
+==== プログラムの構造上の問題点 ====
+基本的な継承関係は以下のようになっている((HrpsysCofgigurator2は省略))．
+  * HrpsysCofgigurator (hrpsys_config.py)
+  * HIRONX(HrpsysCofgigurator) (hiro_client.py)
+  * NextageClient(HIRONX) (nextage_clinet.py)
+クラスとしてはHrpsysCofgiguratorがスーパークラスでその下がサブクラスである．
+問題は，HrpsysCofgiguratorがヒューマノイドロボットを含む多数のロボットの機能（メソッド）をいろいろ含んでいて，
+サブクラスではそのサブセットの機能を使う構造になっているという点である．
+**サブクラスなのだから機能もそのサブセットで良いと思ったとしたら大間違い．オブジェクト指向を理解していないと言える．
+((スーパークラスはサブクラスの「汎化」であって，その機能はサブクラスの機能の共通部分を抽象化したものになっていなくてはならない．))
+**
+そのためプログラムを追うことがとても難しくなっている．下から順に上に追っていくに従って，
+どれを読まなくても良いのかよくわからない状態で読まなくても良い他のロボット用のプログラムを読まされることになる．
+とても困ったものである．
 ==== nextage.py ====
@@ 行 130: / 行 151: @@
 データの部分書き換えなどの整合性をとっている．
-__init__関数ではgazeboシミュレータの利用をチェックしている．gazeboを使わない場合はHIRONXの__init__を呼び出す．
+%%__init__関数%%ではgazeboシミュレータの利用をチェックしている．gazeboを使わない場合はHIRONXの%%__init__%%を呼び出す．
 gazeboを使う方の処理は今回は無視する．
@@ 行 292: / 行 313: @@
         HrpsysConfigurator.waitForModelLoader(self)
         print(self.configurator_name + "start hrpsys")
+</code>
+ModelLoderでvrmlモデルのロードを行っている．この部分はシミュレータモデルの構築だけでなく，アームの幾何構造の取得に必要となる．
+<code python>
         print(self.configurator_name + "finding RTCManager and RobotHardware")
@@ 行 307: / 行 331: @@
             import AbsoluteForceSensorService_idl
             import ImpedanceControllerService_idl
+</code>
+qnxで事前に走っていなければならないRTCManagerとRobotHardwareの起動をチェックして，必要な情報を取得している．
+<code python>
         # HrpsysConfigurator.init(self, robotname=robotname, url=url)
         self.sensors = self.getSensors(url)
+</code>
+標準ではnextage-openにはセンサがないのでこれは無視する．
+このようにnextage-open非標準の機能をサポートするコードがtork関係者が参加したプロジェクト用にいろいろ定義されていてコードが読みづらい．
+これはhandに関しても同様．
+<code python>
         # 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()])) :
@@ 行 327: / 行 360: @@
             print(self.configurator_name + "setup hrpsys logger")
             self.setupLogger()
+</code>
+必要とされているrtc（getRTCList）が実行されているかどうかをチェックして
+  * 起動されていれば，findComps
+  * そうでなければ，必要なrtcの生成，接続，活性化，ログの設定（createComps connectComps, activateComps, setupLogger）を行う
+<code python>
         print(self.configurator_name + "setup joint groups for hrpsys controller")
         self.setSelfGroups()
@@ 行 344: / 行 381: @@
 </code>
+seqにGroupsの情報を設定している．なぜ2回繰り返しているのかは不明．
+=== goOffPose ===
+<code python>
+    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)
+</code>
+OffPoseにして，servoOff．
+動作方法の参考になる．
+=== goInitial ===
+<code python>
+    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)
+: 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
+</code>
+nextage_clientのgoInitialから工場出荷設定でないこと（ユーザ設定）を指定して動作させている．
+=== getRTCList ===
+nextage_clientで完全に上書きされている．
+=== setSelfGroups ===
+seqにGroups情報を設定する．HIRONX.intから呼ばれている．
+<code python>
+    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)
+</code>
+=== isCalibDone ===
+servoOnから呼ばれる．
+<code python>
+    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
+</code>
+=== isServoOn ===
+HIRONX.servoOnから呼ばれる．
+<code python>
+    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
+</code>
+=== servoON ===
+いろいろとチェックしている．
+現在位置と目標位置の整合性などもとっているようだ．
+必要な部分だけ抜き出すためにはもう少し解析する必要がある．
+rh_svc.power, rh_svc.servoOnについても調べる必要がある．
+=== servoOff ===
+いろいろとチェックしている．
+現在位置と目標位置の整合性などもとっているようだ．
+必要な部分だけ抜き出すためにはもう少し解析する必要がある．
+rh_svc.power, rh_svc.servoOffについても調べる必要がある．
+=== checkEncoders ===
+要解析．
+=== icまわり ===
+要解析
+=== clearOfGroup ===
+Groupsの設定の解除？
+要解析
 ==== hrpsys_config ====
 <code python>
@@ 行 369: / 行 574: @@
   - 種々の動作制御
 を行っている．
+サブクラスから呼び出されているものおよびさらにそこから呼び出されているものをピックアップする．
+  * connectComps
+    * connectPorts -> rtm?
+    * self.getJointAngleControllerList
+    * rtm.findPort
+  * acctivateComps
+    * self.getRTCInstanceList
+    * rtm.serializeComponents
+  * deactivateComps
+    * self.getRTCInstanceList
+  * createComps
+    * createComp
+    * getRTCList -> nextage
+  * deletComps
+    * deleteComp
+  * findeComps
+    * findComp
+  * connectLoggerPort
+  * setupLogger
+  * goActual
+  * setJointAngle
+  * setJointAngles
+  * setJointAnglesOfGroup
+  * setJointAnglesSequence
+  * setJointAnglesSequenceOfGroup
+  * waitInterpolation
+  * waitInterpolationOfGroup
+  * setInterpolationMode
+  * getJointAngles
+  * getCurrentPose
+  * getCurrentPosition
+  * getCurrentRotation
+  * getCurrentRPY
+  * getReferencePose
+  * getReferencePosition
+  * getReferenceRotation
+  * getReferenceRPY
+  * setTargetPose
+  * setTargetPoseRelative
+  * clear
+  * clearOfGroup
+  * saveLog
+  * clearLog
+  * setMaxLogLength
+  * getActualState
+  * flat2Groups
+上書きされているもの
+  * init
+  * getRTCList
+  * setSelfGroups
+  * isCalibDone
+  * isServoOn
+  * servoOn
+  * servoOff
+  * checkEncoders
 明示的な初期化処理として以下を行っている．ただしこのinitはHIRONXで上書きされているため呼び出されることはない．
@@ 行 420: / 行 683: @@
         self.configurator_name = cname
 </code>
+=== waitForRTCManager ===
+<code python>
+    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))
+</code>
+=== waitForRobotHardware ===
+<code python>
+    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()))
+</code>
+=== getJointAngleControllerList ===
+<code python>
+    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
+</code>
+==== rtm ====
+=== findRTCManager ===
+=== findRTC ===
 ===== nextage_handleに必要な情報 =====
@@ 行 741: / 行 1067: @@
             self.log.start(self.log.owned_ecs[0])
 </code>
+==== キャリブレーション ====
+キャリブレションの実施確認
+hiro_handle:なし
+nextage:
+==== 動作 ====