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