search

tork-a / rtmros_nextage

ROS-OpenRTM-based opensource robot controller software for dual-armed robot Nextage from Kawada Industries
http://wiki.ros.org/rtmros_nextage
27 stars 39 forks source link

Issue #20 persists when hrpsys version is old inside the robot (AttributeError: 'NoneType' object has no attribute 'get_port_profile') #40

Closed 130s closed 10 years ago

130s commented 10 years ago

Solution in issue #20 might not work when the version of hrpsys inside the robot does not contain this patch or other related changes if any (newly introduced ports do not exist in the robot). And considering there are cases where robot's internal binary can not often be updated, we need a workaround on client's side.

Following is a quick patch toward hironx_ros_bridge/hironx_client.py by @emijah. This overrides connectComps from HrpsysConfigurator and removes portion regarding the non-existing ports. He says robots of Hiro-type don't use these new ports anyway and thus patched there. He might share more ideal solution idea.

--- /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py.org   2014-01-31 11:47:44.638244752 +0900
+++ /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py   2014-01-31 11:48:15.050245282 +0900
@@ -638,3 +638,144 @@
         @return: What RobotHardware.writeDigitalOutput returns (TODO: document)
         '''
         HrpsysConfigurator.writeDigitalOutputWithMask(self, dout, mask)
+
+    # public method
+    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"))
+
+        # 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.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 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"))
emijah commented 10 years ago

One thing you can do, is use the following condition to check for port names before actually connecting.

if 'tqRef' in self.seq.ports.keys() and 'tqIn' in self.sh.ports.keys():
    connectPorts(self.seq.port("tqRef"), self.sh.port("tqIn"))
k-okada commented 10 years ago

We should try not to write robot-depends codes as much as possible, that's is the key to create more robust code, as Linus's Law "given enough eyeballs, all bugs are shallow".

So my proposal is to add following codes in rtm.py so that we can avoid similar problem in the future.

$ svn diff rtm.py
Index: rtm.py
===================================================================
--- rtm.py  (リビジョン 959)
+++ rtm.py  (作業コピー)
@@ -446,7 +446,13 @@
 def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000):
    if not isinstance(inPs, list):
        inPs = [inPs]
+        if not outP:
+                print '[rtm.py] \033[31m   Failed to connect %s to %s\033[0m'%(outP, [inP.get_port_profile().name if inP else inP for inP in inPs])
+                return
    for inP in inPs: 
+                if not inP:
+                        print '[rtm.py] \033[31m   Failed to connect %s to %s\033[0m'%(outP.get_port_profile().name, inP)
+                        continue
        if isConnected(outP, inP) == True:
            print outP.get_port_profile().name,'and',inP.get_port_profile().name,'are already connected'
130s commented 10 years ago

We should try not to write robot-depends codes as much as possible, that's is the key to create more robust code, as Linus's Law "given enough eyeballs, all bugs are shallow".

+1000

@emijah is on this

emijah commented 10 years ago

I agree that this would be a more robust solution.

I have confirmed this works. Output follows.

nextage@visionpc:~$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nxc100 --robot RobotHardware0 --port 15005 --modelfile /opt/jsk/etc/HIRONX/model/main.wrl
Python 2.7.3 (default, Sep 26 2013, 20:03:06) 
Type "copyright", "credits" or "license" for more information.

IPython 0.12.1 -- An enhanced Interactive Python.
?         -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help      -> Python's own help system.
object?   -> Details about 'object', use 'object??' for extra details.
configuration ORB with  nxc100 : 15005
[hrpsys.py]  waiting ModelLoader
[hrpsys.py]  start hrpsys
[hrpsys.py]  finding RTCManager and RobotHardware
[hrpsys.py]  wait for RTCmanager :  None
[hrpsys.py]  wait for RobotHardware0  :  <hrpsys.rtm.RTcomponent instance at 0x297ea28> (timeout  0  < 10)
[hrpsys.py]  findComps -> RobotHardware :  <hrpsys.rtm.RTcomponent instance at 0x297ea28>
[hrpsys.py]  simulation_mode :  False
[hrpsys.py]    bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl
[hrpsys.py]  creating components
[hrpsys.py]    eval :  [self.seq, self.seq_svc] = self.createComp("SequencePlayer","seq")
[hrpsys.py]  create Comp ->  SequencePlayer  :  <hrpsys.rtm.RTcomponent instance at 0x29813b0>
[hrpsys.py]  create CompSvc ->  SequencePlayer Service :  <OpenHRP._objref_SequencePlayerService instance at 0x298c7e8>
[hrpsys.py]    eval :  [self.sh, self.sh_svc] = self.createComp("StateHolder","sh")
[hrpsys.py]  create Comp ->  StateHolder  :  <hrpsys.rtm.RTcomponent instance at 0x2981050>
[hrpsys.py]  create CompSvc ->  StateHolder Service :  <OpenHRP._objref_StateHolderService instance at 0x2987dd0>
[hrpsys.py]    eval :  [self.fk, self.fk_svc] = self.createComp("ForwardKinematics","fk")
[hrpsys.py]  create Comp ->  ForwardKinematics  :  <hrpsys.rtm.RTcomponent instance at 0x29815f0>
[hrpsys.py]  create CompSvc ->  ForwardKinematics Service :  <OpenHRP._objref_ForwardKinematicsService instance at 0x297bc68>
[hrpsys.py]    eval :  [self.el, self.el_svc] = self.createComp("SoftErrorLimiter","el")
[hrpsys.py]  create Comp ->  SoftErrorLimiter  :  <hrpsys.rtm.RTcomponent instance at 0x29837a0>
[hrpsys.py]  create CompSvc ->  SoftErrorLimiter Service :  <OpenHRP._objref_SoftErrorLimiterService instance at 0x298cc68>
[hrpsys.py]    eval :  [self.sc, self.sc_svc] = self.createComp("ServoController","sc")
[hrpsys.py]  create Comp ->  ServoController  :  <hrpsys.rtm.RTcomponent instance at 0x2983518>
[hrpsys.py]  create CompSvc ->  ServoController Service :  <OpenHRP._objref_ServoControllerService instance at 0x2986f80>
[hrpsys.py]    eval :  [self.log, self.log_svc] = self.createComp("DataLogger","log")
[hrpsys.py]  create Comp ->  DataLogger  :  <hrpsys.rtm.RTcomponent instance at 0x2983b48>
[hrpsys.py]  create CompSvc ->  DataLogger Service :  <OpenHRP._objref_DataLoggerService instance at 0x29891b8>
[hrpsys.py]  connecting components
[rtm.py]    Connect sh.qOut - el.qRef
[rtm.py]    Connect el.q - RobotHardware0.qRef
[rtm.py]    Connect RobotHardware0.servoState - el.servoStateIn
[rtm.py]    Connect RobotHardware0.q - sh.currentQIn
[rtm.py]    Connect RobotHardware0.q - fk.q
[rtm.py]    Connect sh.qOut - fk.qRef
[rtm.py]    Connect seq.qRef - sh.qIn
[rtm.py]    Failed to connect None to [None]
[rtm.py]    Connect seq.basePos - sh.basePosIn
[rtm.py]    Connect seq.baseRpy - sh.baseRpyIn
[rtm.py]    Connect seq.zmpRef - sh.zmpIn
[rtm.py]    Connect sh.basePosOut - seq.basePosInit
[rtm.py]    Connect sh.basePosOut - fk.basePosRef
[rtm.py]    Connect sh.baseRpyOut - seq.baseRpyInit
[rtm.py]    Connect sh.baseRpyOut - fk.baseRpyRef
[rtm.py]    Connect sh.qOut - seq.qInit
[rtm.py]    Connect RobotHardware0.q - el.qCurrent
[hrpsys.py]  activating components
[hrpsys.py]    setupLogger : record type = TimedDoubleSeq , name =  q  to  RobotHardware0_q
[rtm.py]    Connect RobotHardware0.q - log.RobotHardware0_q
[hrpsys.py]    setupLogger : record type = TimedDoubleSeq , name =  tau  to  RobotHardware0_tau
[rtm.py]    Connect RobotHardware0.tau - log.RobotHardware0_tau
[hrpsys.py]  sensor names for DataLogger
[hrpsys.py]    setupLogger : record type = TimedDoubleSeq , name =  qRef  to  seq_qRef
[rtm.py]    Connect seq.qRef - log.seq_qRef
[hrpsys.py]    setupLogger : record type = TimedDoubleSeq , name =  qOut  to  sh_qOut
[rtm.py]    Connect sh.qOut - log.sh_qOut
[hrpsys.py]    setupLogger : record type = TimedPoint3D , name =  basePosOut  to  sh_basePosOut
[rtm.py]    Connect sh.basePosOut - log.sh_basePosOut
[hrpsys.py]    setupLogger : record type = TimedOrientation3D , name =  baseRpyOut  to  sh_baseRpyOut
[rtm.py]    Connect sh.baseRpyOut - log.sh_baseRpyOut
[hrpsys.py]    setupLogger :  emergencySignal  arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.emergencySignal - log.emergencySignal
[hrpsys.py]  setup logger done
[hrpsys.py]  initialized successfully

In [1]: 
Do you really want to exit ([y]/n)?
emijah commented 10 years ago

The error output could probably be improved. It will be a bit of a drastic change, but if you change the arguments for connectPorts to be connectPorts(self.rh, 'q', self.tc, 'qCurrent') Then you can capture the error within the function and return the RTC name and port name combination that were the culprits. Or you could add a new function "connectPort" and depreciate the connectPorts interface.

emijah commented 10 years ago

I've modified connectPorts and wrote a function called connectPort.

The output is now changed to this.

seq.qRef and sh.qIn are already connected
[rtm.py]    Failed to connect seq.tqRef to sh.tqIn
seq.basePos and sh.basePosIn are already connected

I modified connectPorts to create a function connectPort. I also modified connectComps which is also shown below.

def connectPort(r1, r1port, r2, r2port, subscription="flush", dataflow="Push", bufferlength=1, rate=1000):
    outP = r1.port(r1port)
    inP = r2.port(r2port)
    if not outP:
        print '[rtm.py] \033[31m   Failed to connect %s.%s to %s.%s\033[0m'%(r1.name(), r1port,
                                                                r2.name(), r2port)
        return
    elif not inP:
        print '[rtm.py] \033[31m   Failed to connect %s to %s\033[0m'%(outP.get_port_profile().name, inP)
    elif isConnected(outP, inP) == True:
        print outP.get_port_profile().name,'and',inP.get_port_profile().name,'are already connected'
    elif dataTypeOfPort(outP) != dataTypeOfPort(inP):
        print outP.get_port_profile().name,'and',inP.get_port_profile().name,'have different data types'
    else:
        nv1 = SDOPackage.NameValue("dataport.interface_type",
                                           any.to_any("corba_cdr"))
                nv2 = SDOPackage.NameValue("dataport.dataflow_type",
                                           any.to_any(dataflow))
                nv3 = SDOPackage.NameValue("dataport.subscription_type",
                                           any.to_any(subscription))
        nv4 = SDOPackage.NameValue("dataport.buffer.length",
                                           any.to_any(str(bufferlength)))
        nv5 = SDOPackage.NameValue("dataport.publisher.push_rate",
                                           any.to_any(str(rate)))
        nv6 = SDOPackage.NameValue("dataport.data_type",
                                           any.to_any(dataTypeOfPort(outP)))
            con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP],
                                                [nv1, nv2, nv3, nv4, nv5, nv6])
            print '[rtm.py]    Connect ' + outP.get_port_profile().name + ' - ' + inP.get_port_profile().name
            ret,prof = inP.connect(con_prof)
            if ret != RTC.RTC_OK:
            print "failed to connect"
                # confirm connection                                                                                      
                elif isConnected(outP, inP) == False:
                        print "connect() returned RTC_OK, but not connected"

I also modified connectComps as follows.

    # public method                                                                                                       
    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:
            connectPort(self.sh, "qOut",  tmp_contollers[0], "qRef")
            for i in range(len(tmp_contollers)-1):
                connectPort(tmp_contollers[i], "q", tmp_contollers[i+1], "qRef")
            if self.simulation_mode:
                connectPort(tmp_contollers[-1], "q",  self.hgc, "qIn")
                connectPort(self.hgc, "qOut", self.rh, "qRef")
            else :
                connectPort(tmp_contollers[-1], "q",  self.rh, "qRef")
        else:
            if self.simulation_mode :
                connectPort(self.sh, "qOut",  self.hgc, "qIn")
                connectPort(self.hgc, "qOut", self.rh, "qRef")
            else:
                connectPort(self.sh, "qOut",  self.rh, "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
                connectPort(self.rh, s_acc[0].name, self.kf, '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
                connectPort(self.rh, s_rate[0].name, self.kf, "rate")
            connectPort(self.seq, "accRef", self.kf, "accRef")

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

        # connection for sh, seq, fk                                                                                      
        #connectPort(self.rh.port("q"), [self.sh.port("currentQIn"), self.fk.port("q")]) # connection for actual joint an\
gles                                                                                                                      
        connectPort(self.rh, "q", self.sh, "currentQIn")
        connectPort(self.rh, "q", self.fk, "q")

        connectPort(self.sh,  "qOut",    self.fk, "qRef")
        connectPort(self.seq, "qRef",    self.sh, "qIn")
        connectPort(self.seq, "tqRef",   self.sh, "tqIn")
        connectPort(self.seq, "basePos", self.sh, "basePosIn")
        connectPort(self.seq, "baseRpy", self.sh, "baseRpyIn")
        connectPort(self.seq, "zmpRef",  self.sh, "zmpIn")
        #connectPort(self.sh.port("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")])              
        connectPort(self.sh, "basePosOut", self.seq, "basePosInit")
        connectPort(self.sh, "basePosOut", self.fk,  "basePosRef")
        #connectPort(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")])              
        connectPort(self.sh, "baseRpyOut", self.seq, "baseRpyInit")
        connectPort(self.sh, "baseRpyOut", self.fk,  "baseRpyRef")
        connectPort(self.sh, "qOut",       self.seq, "qInit")

        # connection for st                                                                                               
        if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(self.rh.ref, "rfsensor") and self.st:
            connectPort(self.rh,  "lfsensor", self.st, "forceL")
            connectPort(self.rh,  "rfsensor", self.st, "forceR")
            connectPort(self.kf,  "rpy",      self.st, "rpy")
            connectPort(self.abc, "zmpRef",   self.st, "zmpRef")
            connectPort(self.abc, "baseRpy",  self.st, "baseRpyIn")
            connectPort(self.abc, "basePos",  self.st, "basePosIn")
        if self.ic and self.abc:
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPort(self.ic, "ref_"+sen.name, self.abc, "ref_"+sen.name)

        #  actual force sensors                                                                                           
        if self.afs and self.kf:
            #connectPort(self.kf.port("rpy"), self.ic.port("rpy"))                                                        
            connectPort(self.kf, "rpy", self.afs, "rpy")
            connectPort(self.rh, "q",   self.afs, "qCurrent")
            for sen in filter(lambda x : x.type == "Force", self.sensors):
                connectPort(self.rh, sen.name, self.afs, sen.name)
                if self.ic:
                    connectPort(self.afs, "off_"+sen.name, self.ic, sen.name)
        # connection for ic                                                                                               
        if self.ic:
            connectPort(self.rh, "q", self.ic, "qCurrent")
        # connection for tf                                                                                               
        if self.tf:
            # connection for actual torques                                                                               
            if rtm.findPort(self.rh.ref, "tau") != None:
                connectPort(self.rh, "tau", self.tf, "tauIn")
            connectPort(self.rh, "q", self.tf, "qCurrent")
        # connection for vs                                                                                               
        if self.vs:
            connectPort(self.rh, "q",      self.vs, "qCurrent")
            connectPort(self.tf, "tauOut", self.vs, "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()):
                    connectPort(self.vs, vfp, self.ic, vfp)
        # connection for co                                                                                               
        if self.co:
            connectPort(self.rh, "q", self.co, "qCurrent")

        # connection for gc                                                                                               
        if self.gc:
            connectPort(self.rh, "q", self.gc, "qCurrent") # other connections                                            
            tmp_controller = self.getJointAngleControllerList()
            index = tmp_controller.index(self.gc)
            if index == 0:
                connectPort(self.sh, "qOut", self.gc, "qIn")
            else:
                connectPort(tmp_controller[index - 1], "q", self.gc, "qIn")

        # connection for te                                                                                               
        if self.te:
            if self.tf:
                connectPort(self.tf, "tauOut", self.te, "tauIn")
            else:
                connectPort(self.rh, "tau",    self.te, "tauIn")
            # sevoStateIn is connected above

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

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

        # connection for el                                                                                               
        if self.el:
            connectPort(self.rh, "q",  self.el, "qCurrent")
k-okada commented 10 years ago

I thought about similar idea but we also need to find other files that uses connectPort function[1]. If Python has macro, we can write as follows:

(defun connectPort1 (out in)
  (format t "connectPort1 ~A ~A~%" out in)
  (unless out (format t "~A is not defined~%" out)))

(defmacro connectPort2 (out in)
  `(progn (format t "connectPort2 ~A ~A~%" out in)
          (unless ,out (format t "~A is not defined~%" out))))

(setq a nil)
(setq b "q")

(connectPort1 a b)
(connectPort2 a b)

(print (macroexpand '(connectPort2 a b)))

this outputs

connectPort1 nil q
nil is not defined
connectPort2 a b
a is not defined
(progn (format t "connectPort2 ~A ~A~%" out in) (unless a (format t "~A is not \
defined~%" out)))

[1] http://code.google.com/p/rtm-ros-robotics/source/browse/trunk/rtmros_common/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py

k-okada commented 10 years ago

上記修正を加えてhrpsys-baseを315.1.7にしました．

emijah commented 10 years ago

Actually I think the lisp macro equivalent would be python decorator functions. I'll try to implement something for future reference.

130s commented 10 years ago

I confirmed that the patch in hrpsys built from source works on simulator.

emijah commented 10 years ago

Found out that using decorators is not possible because the arguments are already None when entering the function. Therefore it is impossible to get the port names with the current argument list.