[python/hrpsys_config.py, launch/samplerobot.launch] enable to use rf…

…u in robots
fkanehiro · Apr 11, 2016 · 872b832 · 872b832
1 parent 7fcb991
commit 872b832
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 15 deletions.
diff --git a/launch/samplerobot.launch b/launch/samplerobot.launch
@@ -56,6 +56,7 @@
               -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)"
               -o "example.CollisionDetector.config_file:$(arg CONF_FILE)"
               -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)"
+              -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)"
               -o "example.$(arg JOINT_CONTROLLER).config_file:$(arg CONF_FILE)"
               -o "corba.nameservers:localhost:$(arg corbaport)"
               $(arg hrpsys_gui_args)'

diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py
@@ -252,6 +252,11 @@ class HrpsysConfigurator:
     bp_svc = None
     bp_version = None
 
+    # ReferenceForceUpdater
+    rfu = None
+    rfu_svc = None
+    rfu_version = None
+
     # rtm manager
     ms = None
 
@@ -387,28 +392,33 @@ def connectComps(self):
 
         # ref force moment connection
         for sen in self.getForceSensorNames():
-            if self.st:
+            if self.abc and self.st:
                 connectPorts(self.abc.port(sen),
                              self.st.port(sen + "Ref"))
+                connectPorts(self.abc.port("limbCOPOffset_"+sen),
+                             self.st.port("limbCOPOffset_"+sen))
+            if self.rfu:
+                ref_force_port_from = self.rfu.port("ref_"+sen+"Out")
+            elif self.es:
+                ref_force_port_from = self.es.port(sen+"Out")
+            else:
+                ref_force_port_from = self.sh.port(sen+"Out")
+            if self.ic:
+                connectPorts(ref_force_port_from,
+                             self.ic.port("ref_" + sen+"In"))
+            if self.abc:
+                connectPorts(ref_force_port_from,
+                             self.abc.port("ref_" + sen))
             if self.es:
                 connectPorts(self.sh.port(sen+"Out"),
-                                 self.es.port(sen+"In"))
-                if self.ic:
-                    connectPorts(self.es.port(sen+"Out"),
-                                 self.ic.port("ref_" + sen+"In"))
-                if self.abc:
+                             self.es.port(sen+"In"))
+                if self.rfu:
                     connectPorts(self.es.port(sen+"Out"),
-                                 self.abc.port("ref_" + sen))
+                                 self.rfu.port("ref_" + sen+"In"))
             else:
-                if self.ic:
-                    connectPorts(self.sh.port(sen+"Out"),
-                                 self.ic.port("ref_" + sen+"In"))
-                if self.abc:
+                if self.rfu:
                     connectPorts(self.sh.port(sen+"Out"),
-                                 self.abc.port("ref_" + sen))
-            if self.abc and self.st:
-                connectPorts(self.abc.port("limbCOPOffset_"+sen),
-                             self.st.port("limbCOPOffset_"+sen))
+                                 self.rfu.port("ref_" + sen+"In"))
 
         #  actual force sensors
         if self.rmfo:
@@ -421,6 +431,9 @@ def connectComps(self):
                 if self.ic:
                     connectPorts(self.rmfo.port("off_" + sen.name),
                                  self.ic.port(sen.name))
+                if self.rfu:
+                    connectPorts(self.rmfo.port("off_" + sen.name),
+                                 self.rfu.port(sen.name))
                 if self.st:
                     connectPorts(self.rmfo.port("off_" + sen.name),
                                  self.st.port(sen.name))
@@ -435,6 +448,13 @@ def connectComps(self):
             if self.seq_version >= '315.3.0':
                 connectPorts(self.sh.port("basePosOut"), self.ic.port("basePosIn"))
                 connectPorts(self.sh.port("baseRpyOut"), self.ic.port("baseRpyIn"))
+        # connection for rfu
+        if self.rfu:
+            if self.es:
+                connectPorts(self.es.port("q"), self.rfu.port("qRef"))
+            if self.seq_version >= '315.3.0':
+                connectPorts(self.sh.port("basePosOut"), self.rfu.port("basePosIn"))
+                connectPorts(self.sh.port("baseRpyOut"), self.rfu.port("baseRpyIn"))
         # connection for tf
         if self.tf:
             # connection for actual torques
@@ -685,6 +705,7 @@ def getRTCListUnstable(self):
             ['vs', "VirtualForceSensor"],
             ['rmfo', "RemoveForceSensorLinkOffset"],
             ['es', "EmergencyStopper"],
+            ['rfu', "ReferenceForceUpdater"],
             ['ic', "ImpedanceController"],
             ['abc', "AutoBalancer"],
             ['st', "Stabilizer"],