Exemplo n.º 1
0
 def connectLoggerPort(self, artc, sen_name, log_name=None):
     log_name = log_name if log_name else artc.name() + "_" + sen_name
     if artc and rtm.findPort(artc.ref, sen_name) != None:
         sen_type = rtm.dataTypeOfPort(artc.port(sen_name)).split("/")[1].split(":")[0]
         if rtm.findPort(self.log.ref, log_name) == None:
             print self.configurator_name, "  setupLogger : record type =", sen_type, ", name = ", sen_name, " to ", log_name
             self.log_svc.add(sen_type, log_name)
         else:
             print self.configurator_name, "  setupLogger : ", sen_name, " arleady exists in DataLogger"
         connectPorts(artc.port(sen_name), self.log.port(log_name))
Exemplo n.º 2
0
 def connectLoggerPort(self, artc, sen_name, log_name=None):
     log_name = log_name if log_name else artc.name() + "_" + sen_name
     if artc and rtm.findPort(artc.ref, sen_name) != None:
         sen_type = rtm.dataTypeOfPort(artc.port(sen_name)).split("/")[1].split(":")[0]
         if rtm.findPort(self.log.ref, log_name) == None:
             print self.configurator_name, "  setupLogger : record type =", sen_type, ", name = ", sen_name, " to ", log_name
             self.log_svc.add(sen_type, log_name)
         else:
             print self.configurator_name, "  setupLogger : ", sen_name, " arleady exists in DataLogger"
         connectPorts(artc.port(sen_name), self.log.port(log_name))
Exemplo n.º 3
0
    def checkSimulationMode(self):
        # distinguish real robot from simulation by using "servoState" port
        if rtm.findPort(self.rh.ref, "servoState") == None:
            self.hgc = findRTC("HGcontroller0")
            self.simulation_mode = True
        else:
            self.simulation_mode = False
            self.rh_svc = narrow(self.rh.service("service0"), "RobotHardwareService")
            self.ep_svc = narrow(self.rh.ec, "ExecutionProfileService")

        print self.configurator_name, "simulation_mode : ", self.simulation_mode
Exemplo n.º 4
0
    def checkSimulationMode(self):
        # distinguish real robot from simulation by using "servoState" port
        if rtm.findPort(self.rh.ref, "servoState") == None:
            self.hgc = findRTC("HGcontroller0")
            self.simulation_mode = True
        else:
            self.simulation_mode = False
            self.rh_svc = narrow(self.rh.service("service0"), "RobotHardwareService")
            self.ep_svc = narrow(self.rh.ec, "ExecutionProfileService")

        print self.configurator_name, "simulation_mode : ", self.simulation_mode
Exemplo n.º 5
0
    def connectComps(self):
        if self.rh == None or self.seq == None or self.sh == None or self.fk == None:
            print self.configurator_name, "\e[1;31m connectComps : hrpsys requries rh, seq, sh and fk, please check rtcd.conf or rtcd arguments\e[0m"
            return
        # connection for reference joint angles
        tmp_contollers = self.getJointAngleControllerList()
        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:
            if self.te and self.el:
                connectPorts(self.rh.port("servoState"), self.te.port("servoStateIn"))
                connectPorts(self.te.port("servoStateOut"), self.el.port("servoStateIn"))
            elif self.el:
                connectPorts(self.rh.port("servoState"), self.el.port("servoStateIn"))
            elif self.te:
                connectPorts(self.rh.port("servoState"), self.te.port("servoStateIn"))

        # connection for sh, seq, 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"))
        connectPorts(self.seq.port("qRef"), self.sh.port("qIn"))
        connectPorts(self.seq.port("tqRef"), self.sh.port("tqIn"))
        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("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")])
        connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")])
        connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))
        connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit"))
        for sen in filter(lambda x : x.type == "Force", self.sensors):
            connectPorts(self.seq.port(sen.name+"Ref"), self.sh.port(sen.name+"In"))

        # 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"))
        if self.ic and self.abc:
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPorts(self.ic.port("ref_"+sen.name), self.abc.port("ref_"+sen.name))

        #  actual force sensors
        if self.rmfo and self.kf:
            #connectPorts(self.kf.port("rpy"), self.ic.port("rpy"))
            connectPorts(self.kf.port("rpy"), self.rmfo.port("rpy"))
            connectPorts(self.rh.port("q"), self.rmfo.port("qCurrent"))
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPorts(self.rh.port(sen.name), self.rmfo.port(sen.name))
                if self.ic:
                    connectPorts(self.rmfo.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 gc
        if self.gc:
            connectPorts(self.rh.port("q"), self.gc.port("qCurrent")) # other connections
            tmp_controller = self.getJointAngleControllerList()
            index = tmp_controller.index(self.gc)
            if index == 0:
                connectPorts(self.sh.port("qOut"), self.gc.port("qIn"))
            else:
                connectPorts(tmp_controller[index - 1].port("q"), self.gc.port("qIn"))

        # connection for te
        if self.te:
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.te.port("tauIn"))
            else:
                connectPorts(self.rh.port("tau"), self.te.port("tauIn"))
            # sevoStateIn is connected above
            
        # connection for tl
        if self.tl:
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.tl.port("tauIn"))
            else:
                connectPorts(self.rh.port("tau"), self.tl.port("tauIn"))
            if self.te:
                connectPorts(self.te.port("tempOut"), self.tl.port("tempIn"))
            connectPorts(self.rh.port("q"), self.tl.port("qCurrentIn"))
            # qRef is connected as joint angle controller

        # connection for tc
        if self.tc:
            connectPorts(self.rh.port("q"), self.tc.port("qCurrent"))
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.tc.port("tauCurrent"))
            else:
                connectPorts(self.rh.port("tau"), self.tc.port("tauCurrent"))
            if self.tl:
                connectPorts(self.tl.port("tauMax"), self.tc.port("tauMax"))

        # connection for el
        if self.el:
            connectPorts(self.rh.port("q"), self.el.port("qCurrent"))
Exemplo n.º 6
0
    def connectComps(self):
        if self.rh == None or self.seq == None or self.sh == None or self.fk == None:
            print self.configurator_name, "\e[1;31m connectComps : hrpsys requries rh, seq, sh and fk, please check rtcd.conf or rtcd arguments\e[0m"
            return
        # connection for reference joint angles
        tmp_contollers = self.getJointAngleControllerList()
        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:
            if self.te and self.el:
                connectPorts(self.rh.port("servoState"), self.te.port("servoStateIn"))
                connectPorts(self.te.port("servoStateOut"), self.el.port("servoStateIn"))
            elif self.el:
                connectPorts(self.rh.port("servoState"), self.el.port("servoStateIn"))
            elif self.te:
                connectPorts(self.rh.port("servoState"), self.te.port("servoStateIn"))

        # connection for sh, seq, 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"))
        connectPorts(self.seq.port("qRef"), self.sh.port("qIn"))
        connectPorts(self.seq.port("tqRef"), self.sh.port("tqIn"))
        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("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")])
        connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")])
        connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))
        connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit"))
        for sen in filter(lambda x : x.type == "Force", self.sensors):
            connectPorts(self.seq.port(sen.name+"Ref"), self.sh.port(sen.name+"In"))

        # 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"))
        if self.ic and self.abc:
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPorts(self.ic.port("ref_"+sen.name), self.abc.port("ref_"+sen.name))

        #  actual force sensors
        if self.rmfo and self.kf:
            #connectPorts(self.kf.port("rpy"), self.ic.port("rpy"))
            connectPorts(self.kf.port("rpy"), self.rmfo.port("rpy"))
            connectPorts(self.rh.port("q"), self.rmfo.port("qCurrent"))
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPorts(self.rh.port(sen.name), self.rmfo.port(sen.name))
                if self.ic:
                    connectPorts(self.rmfo.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 gc
        if self.gc:
            connectPorts(self.rh.port("q"), self.gc.port("qCurrent")) # other connections
            tmp_controller = self.getJointAngleControllerList()
            index = tmp_controller.index(self.gc)
            if index == 0:
                connectPorts(self.sh.port("qOut"), self.gc.port("qIn"))
            else:
                connectPorts(tmp_controller[index - 1].port("q"), self.gc.port("qIn"))

        # connection for te
        if self.te:
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.te.port("tauIn"))
            else:
                connectPorts(self.rh.port("tau"), self.te.port("tauIn"))
            # sevoStateIn is connected above
            
        # connection for tl
        if self.tl:
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.tl.port("tauIn"))
            else:
                connectPorts(self.rh.port("tau"), self.tl.port("tauIn"))
            if self.te:
                connectPorts(self.te.port("tempOut"), self.tl.port("tempIn"))
            connectPorts(self.rh.port("q"), self.tl.port("qCurrentIn"))
            # qRef is connected as joint angle controller

        # connection for tc
        if self.tc:
            connectPorts(self.rh.port("q"), self.tc.port("qCurrent"))
            if self.tf:
                connectPorts(self.tf.port("tauOut"), self.tc.port("tauCurrent"))
            else:
                connectPorts(self.rh.port("tau"), self.tc.port("tauCurrent"))
            if self.tl:
                connectPorts(self.tl.port("tauMax"), self.tc.port("tauMax"))

        # connection for el
        if self.el:
            connectPorts(self.rh.port("q"), self.el.port("qCurrent"))