hrpsys_config.pyのconnectComps関数: 想定されている最小構成は？

[k-okada]

From emijah.s on August 22, 2013 18:37:02

現在最小構成で起動しようとしたところ、いろいろとカバーしていない条件がありましたので、変更しました。 想定した最小構成はRobotHardware, StateHolder, SequencePlayerなのですが、現在sh.port('qOut')とrh.port('qRef')を接続するとrtcdが落ちてしまいます。原因究明中です。

# public method
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:
            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"))

    if not self.fk:
        print 'fk does not exist'
        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"))
    else:
        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"))
        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")])

    # 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"))

Original issue: http://code.google.com/p/rtm-ros-robotics/issues/detail?id=172

[k-okada]

From emijah.s on August 22, 2013 02:38:48

さらに最小構成のRobotHardwareとStateHolderもうまく行かず、謎です。

[k-okada]

From kei.ok...@gmail.com on August 22, 2013 02:41:28

https://code.google.com/p/hrpsys-base/issues/detail?id=130 の問題でしょうか？直したもので試してみてください．

Owner: kei.ok...@gmail.com

[k-okada]

From emijah.s on August 22, 2013 17:09:43

ありがとうございます。試してみます。

[k-okada]

From kei.ok...@gmail.com on January 30, 2014 03:00:51

Status: Fixed