search

fkanehiro / hrpsys-base

Basic RT components and utilities to control robots using OpenRTM
Other
41 stars 88 forks source link

hrpsys.pyでコンポーネントが1つの時の挙動がおかしい。 #130

Closed fkanehiro closed 9 years ago

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 21, 2013 12:58:51

以下の部分、これでOKでしょうか？だれかダブルチェックしてください。

leus@parsnip:~/ros/fuerte/rtm-ros-robotics/rtmros_common/hrpsys/scripts$ svn diff

Index: hrpsys.py

--- hrpsys.py (リビジョン 4685) +++ hrpsys.py (作業コピー) @@ -146,11 +146,15 @@ connectPorts(self.sh.port("qOut"), tmp_contollers[0].port("qRef")) for i in range(len(tmp_contollers)-1): connectPorts(tmp_contollers[i].port("q"), tmp_contollers[i+1].port("qRef"))

Original issue: http://code.google.com/p/hrpsys-base/issues/detail?id=130

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 20, 2013 22:00:22

https://code.google.com/p/rtm-ros-robotics/source/detail?spec=svn4637&r=4637 でエンバグしたんだと思います．確認してもらえると嬉しいです＞notheworldさん

別海としていかがあるかな？

    tmp_contollers = filter(lambda c : c != None, [self.ic, self.abc, self.st, self.co, self.el])
    if self.simulation_mode :
        tmp_controllers.append(self.hgc)
    if len(tmp_contollers) > 0:
        connectPorts(self.sh.port("qOut"),  tmp_contollers[0].port("qRef"))
        for i in range(len(tmp_contollers)-1):
            connectPorts(tmp_contollers[i].port("q"), tmp_contollers[i+1].port("qRef"))
        connectPorts(tmp_contollers[-1].port("q"),  self.rh.port("qRef"))

Owner: nothewo...@gmail.com

fkanehiro commented 10 years ago

From nothewo...@gmail.com on August 20, 2013 22:33:06

コンポーネントが一つな状況はどのようなもので再現できるでしょうか。 以下のようにgetRTCListを一個にするかんじでしょうか。 以下はSEQだけにしてみています。

def getRTCList(self):
    return [
        ['seq', "SequencePlayer"]
        # ['sh', "StateHolder"],                                                                                                                                            
        # ['fk', "ForwardKinematics"],                                                                                                                                      
        # ['tf', "TorqueFilter"],                                                                                                                                           
        # ['kf', "KalmanFilter"],                                                                                                                                           
        # ['vs', "VirtualForceSensor"],                                                                                                                                     
        # ['afs', "AbsoluteForceSensor"],                                                                                                                                   
        # ['ic', "ImpedanceController"],                                                                                                                                    
        # ['abc', "AutoBalancer"],                                                                                                                                          
        # ['st', "Stabilizer"],                                                                                                                                             
        # ['co', "CollisionDetector"],                                                                                                                                      
        # ['el', "SoftErrorLimiter"],                                                                                                                                       
        # ['log', "DataLogger"]                                                                                                                                             
        ]
fkanehiro commented 10 years ago

From nothewo...@gmail.com on August 20, 2013 23:35:26

r818 でsimulation_modeにかんするif文を、 指摘していただいたパッチをもとに修正しました。 ご確認いただけますでしょうか。

別海としていかがあるかな？ こちらは、hgc, shまでふくめてしまうと、 データポートがqInだったりqRefだったり、qだったりqOutだったりして、 もう少し複雑になってしまうと思います。

あと、rtcが１というのは何RTCが一個ということでしょうか。

すくなくともcontroller関係では、 seqとshがペアになってないと正しく動かないきがします。

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 21, 2013 19:54:11

あと、rtcが１というのは何RTCが一個ということでしょうか。

すくなくともcontroller関係では、 seqとshがペアになってないと正しく動かないきがします。

rtcが一個というのは，tmp_controllersにはいっているもおんが一つという意味なので，seq, shはあるうえで，以下のうちのrtcが一つ（hiroの場合はecのみ）という状況です．

tmp_contollers = filter(lambda c : c != None, [self.ic, self.abc, self.st, self.co, self.el])

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 21, 2013 19:56:29

isaacさん，これでhiroが動くか確認お願いできますでしょうか？ roscd hrpsys; make download; makeとして /hrpsys/lib/python2.7/dist-packages/hrpsys_config.py が変わっていることを確認してから，試してみて下さい．

Cc: gm130s

fkanehiro commented 10 years ago

From emijah.s on August 22, 2013 18:58:33

notheworldさん、 r819 ですね。

fkanehiro commented 10 years ago

From nothewo...@gmail.com on August 22, 2013 19:23:52

すいません、 r819 でした。 いずれにしても、hironxにてご確認いただけますと助かります。

fkanehiro commented 10 years ago

From gm130s on August 22, 2013 19:25:12

Hiro 上での確認，了解です．今日 (23F) か 25日になります．

fkanehiro commented 10 years ago

From emijah.s on August 22, 2013 20:09:06

こちらでも試したいと思います。てこずりそうでしたら、そちらにお邪魔します。

fkanehiro commented 10 years ago

From emijah.s on August 22, 2013 22:55:20

一応connectCompsを以下のように変更したら、こちらにあるHIROでRobotHardware+StateHolderという構成で起動することができました。これからSequencePlayer, ServoControllerを追加して行きます。

fkanehiro commented 10 years ago

From emijah.s on August 22, 2013 22:57:40

関数を添付するのを忘れました。

def connectComps(self):
    # connection for reference joint angles
    tmp_contollers = filter(lambda c : c != None, [self.ic, self.abc, self.st, self.co, self.el])
    if len(tmp_contollers) > 0:
        connectPorts(self.sh.port("qOut"),  tmp_contollers[0].port("qRef"))
        for i in range(len(tmp_contollers)-1):
            connectPorts(tmp_contollers[i].port("q"), tmp_contollers[i+1].port("qRef"))
        if self.simulation_mode :
            connectPorts(tmp_contollers[-1].port("q"),  self.hgc.port("qIn"))
            connectPorts(self.hgc.port("qOut"), self.rh.port("qRef"))
        else:
            connectPorts(tmp_contollers[-1].port("q"),  self.rh.port("qRef"))
    else:
        if self.simulation_mode:
            connectPorts(self.sh.port("qOut"),  self.hgc.port("qIn"))
            connectPorts(self.hgc.port("qOut"), self.rh.port("qRef"))
        else:
            raw_input("connectPorts(self.sh.port('qOut'), self.rh.port('qRef')) >")
            connectPorts(self.sh.port('qOut'), self.rh.port('qRef'))

    # connection for kf
    if self.kf:
        #   currently use first acc and rate sensors for kf
        s_acc=filter(lambda s : s.type == 'Acceleration', self.sensors)
        if (len(s_acc)>0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
            connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc'))
        s_rate=filter(lambda s : s.type == 'RateGyro', self.sensors)
        if (len(s_rate)>0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming rule of gsensor and gyrometer
            connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))
        connectPorts(self.seq.port("accRef"), self.kf.port("accRef"))

    # connection for rh
    if self.rh.port("servoState") != None and self.el:
        connectPorts(self.rh.port("servoState"), self.el.port("servoStateIn"))

    # connection for sh, seq, fk
    if self.seq and self.sh:
        connectPorts(self.seq.port("qRef"), self.sh.port("qIn"))
        connectPorts(self.seq.port("basePos"), self.sh.port("basePosIn"))
        connectPorts(self.seq.port("baseRpy"), self.sh.port("baseRpyIn"))
        connectPorts(self.seq.port("zmpRef"),  self.sh.port("zmpIn"))
        connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))

    # connection for sh, seq, fk
    if self.fk:
        connectPorts(self.rh.port("q"), [self.sh.port("currentQIn"), self.fk.port("q")]) # connection for actual joint angles
        connectPorts(self.sh.port("qOut"),  self.fk.port("qRef"))
        if self.seq:
            connectPorts(self.sh.port("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")])
            connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")])
    else:
        raw_input("connectPorts(self.rh.port('q'), self.sh.port('currentQIn")) >")
        connectPorts(self.rh.port("q"), self.sh.port("currentQIn")) # connection for actual joint angles
        if self.seq:
            connectPorts(self.sh.port("basePosOut"), self.seq.port("basePosInit"))
            connectPorts(self.sh.port("baseRpyOut"), self.seq.port("baseRpyInit"))

    if self.sh and self.seq:
        connectPorts(self.seq.port("qRef"), self.sh.port("qIn"))
        connectPorts(self.seq.port("basePos"), self.sh.port("basePosIn"))
        connectPorts(self.seq.port("baseRpy"), self.sh.port("baseRpyIn"))
        connectPorts(self.seq.port("zmpRef"),  self.sh.port("zmpIn"))
        connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))

    # connection for st
    if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(self.rh.ref, "rfsensor") and self.st:
        connectPorts(self.rh.port("lfsensor"), self.st.port("forceL"))
        connectPorts(self.rh.port("rfsensor"), self.st.port("forceR"))
        connectPorts(self.kf.port("rpy"), self.st.port("rpy"))
        connectPorts(self.abc.port("zmpRef"), self.st.port("zmpRef"))
        connectPorts(self.abc.port("baseRpy"), self.st.port("baseRpyIn"))
        connectPorts(self.abc.port("basePos"), self.st.port("basePosIn"))

    #  actual force sensors
    if self.afs and self.kf:
        #connectPorts(self.kf.port("rpy"), self.ic.port("rpy"))
        connectPorts(self.kf.port("rpy"), self.afs.port("rpy"))
        connectPorts(self.rh.port("q"), self.afs.port("qCurrent"))
        for sen in filter(lambda x : x.type == "Force", self.sensors):
            connectPorts(self.rh.port(sen.name), self.afs.port(sen.name))
            if self.ic:
                connectPorts(self.afs.port("off_"+sen.name), self.ic.port(sen.name))
    # connection for ic
    if self.ic:
        connectPorts(self.rh.port("q"), self.ic.port("qCurrent"))
    # connection for tf
    if self.tf:
        # connection for actual torques
        if rtm.findPort(self.rh.ref, "tau") != None:
            connectPorts(self.rh.port("tau"), self.tf.port("tauIn"))
        connectPorts(self.rh.port("q"), self.tf.port("qCurrent"))
    # connection for vs
    if self.vs:
        connectPorts(self.rh.port("q"), self.vs.port("qCurrent"))
        connectPorts(self.tf.port("tauOut"), self.vs.port("tauIn"))
        #  virtual force sensors
        if self.ic:
            for vfp in filter(lambda x : str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, self.vs.ports.keys()):
                connectPorts(self.vs.port(vfp), self.ic.port(vfp))
    # connection for co
    if self.co:
        connectPorts(self.rh.port("q"), self.co.port("qCurrent"))
    # connection for el
    if self.el:
        connectPorts(self.rh.port("q"), self.el.port("qCurrent"))
fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 22, 2013 23:09:08

r819 で動くはずです

fkanehiro commented 10 years ago

From nothewo...@gmail.com on August 23, 2013 01:38:18

fkなどのチェックが足りないところがあったので、 r823 で修正しました。

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on August 28, 2013 06:50:08

別解としていかがあるかな？ こちらは、hgc, shまでふくめてしまうと、 データポートがqInだったりqRefだったり、qだったりqOutだったりして、 もう少し複雑になってしまうと思います。

なるほど．そうですね．これはこれでよいですね． 一方 r823 ですが， http://rtm-ros-robotics.googlecode.com/svn/trunk/rtmros_common/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch でも RobotHardware SequencePlayer DataLogger ForwardKinematics StateHolder は全てのロボットで動くことが期待されているので対応の必要はないと思います． やるならエラーを出してもいいはず．ということで，戻しておきました．( r827 ) また， r828 で，rh,seq,sh,fk,logが無いときにエラーを表示するようにしておきました．

snozawa commented 9 years ago

こちらの内容，対応すべき箇所は対応されているようですので，closeで大丈夫だと思います． @fkanehiroさん closeをお願いできますでしょうか．