Modify setup.py for each robot to deal with urata_hrpsys_config.py separation

hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py

@@ -1,17 +1,21 @@
 #!/usr/bin/env python
 
-from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import *
+import sys
+from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import ChoreonoidHrpsysConfigurator
+from hrpsys_ros_bridge_tutorials.chidori_hrpsys_config import CHIDORIHrpsysConfigurator
+
+class CHIDORICnoidHrpsysConfigurator(ChoreonoidHrpsysConfigurator, CHIDORIHrpsysConfigurator):
+    def __init__(self):
+        CHIDORIHrpsysConfigurator.__init__(self)
 
-class CHIDORI_HrpsysConfigurator(ChoreonoidHrpsysConfigurator):
     def getRTCList (self):
-        ##return self.getRTCListUnstable()
         return [
             ['seq', "SequencePlayer"],
             ['sh', "StateHolder"],
             ['fk', "ForwardKinematics"],
-            #['tf', "TorqueFilter"],
+            # ['tf', "TorqueFilter"],
             ['kf', "KalmanFilter"],
-            #['vs', "VirtualForceSensor"],
+            # ['vs', "VirtualForceSensor"],
             ['rmfo', "RemoveForceSensorLinkOffset"],
             ['es', "EmergencyStopper"],
             ['rfu', "ReferenceForceUpdater"],
@@ -28,19 +32,14 @@ def getRTCList (self):
             ['log', "DataLogger"]
             ]
 
-    def defJointGroups (self):
-        rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']]
-        lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']]
-        self.Groups = [rleg_group, lleg_group]
-
     def startABSTIMP (self):
         self.startAutoBalancer()
         #self.ic_svc.startImpedanceController("larm")
         #self.ic_svc.startImpedanceController("rarm")
         self.startStabilizer()
 
 if __name__ == '__main__':
-    hcf = CHIDORI_HrpsysConfigurator("CHIDORI")
+    hcf = CHIDORICnoidHrpsysConfigurator()
     [sys.argv, connect_constraint_force_logger_ports] = hcf.parse_arg_for_connect_ports(sys.argv)
     if len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2], connect_constraint_force_logger_ports=connect_constraint_force_logger_ports)

hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py

@@ -1,8 +1,13 @@
 #!/usr/bin/env python
 
-from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import *
+import sys
+from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import ChoreonoidHrpsysConfigurator
+from hrpsys_ros_bridge_tutorials.jaxon_blue_hrpsys_config import JAXON_BLUEHrpsysConfigurator
+
+class JAXON_BLUECnoidHrpsysConfigurator(ChoreonoidHrpsysConfigurator, JAXON_BLUEHrpsysConfigurator):
+    def __init__(self):
+        JAXON_BLUEHrpsysConfigurator.__init__(self)
 
-class JAXON_BLUE_HrpsysConfigurator(ChoreonoidHrpsysConfigurator):
     def getRTCList (self):
         ##return self.getRTCListUnstable()
         return [
@@ -28,36 +33,23 @@ def getRTCList (self):
             ['log', "DataLogger"]
             ]
 
-    def defJointGroups (self):
-        rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6']]
-        larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6']]
-        rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']]
-        lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']]
-        head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']]
-        torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']]
-        # rhand_group = ['rhand', ['RARM_F_JOINT0', 'RARM_F_JOINT1']]
-        # lhand_group = ['lhand', ['LARM_F_JOINT0', 'LARM_F_JOINT1']]
-        # range_group = ['range', ['motor_joint']]
-        self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group]
-
     def startABSTIMP (self):
         ### not used on hrpsys
-        # self.el_svc.setServoErrorLimit("motor_joint",   sys.float_info.max)
-        # self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
-        # self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
-        # self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
-        # self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
+        # self.setServoErrorLimit("motor_joint",   sys.float_info.max)
+        # self.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
+        # self.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
+        # self.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
+        # self.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
         ###
         self.startAutoBalancer()
         # Suppress limit over message and behave like real robot that always angle-vector is in seq.
-        # Latter four 0.0 are for hands.
-        self.seq_svc.setJointAngles(self.jaxonResetPose(), 1.0)
+        self.setResetPose(1.0)
         self.ic_svc.startImpedanceController("larm")
         self.ic_svc.startImpedanceController("rarm")
         self.startStabilizer()
 
 if __name__ == '__main__':
-    hcf = JAXON_BLUE_HrpsysConfigurator("JAXON_BLUE")
+    hcf = JAXON_BLUECnoidHrpsysConfigurator()
     [sys.argv, connect_constraint_force_logger_ports] = hcf.parse_arg_for_connect_ports(sys.argv)
     if len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2], connect_constraint_force_logger_ports=connect_constraint_force_logger_ports)

hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py

@@ -1,8 +1,10 @@
 #!/usr/bin/env python
 
-from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import *
+import sys
+from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import ChoreonoidHrpsysConfigurator
+from hrpsys_ros_bridge_tutorials.jaxon_red_hrpsys_config import JAXON_REDHrpsysConfigurator
 
-class JAXON_RED_HrpsysConfigurator(ChoreonoidHrpsysConfigurator):
+class JAXON_REDCnoidHrpsysConfigurator(ChoreonoidHrpsysConfigurator, JAXON_REDHrpsysConfigurator):
     def getRTCList (self):
         ##return self.getRTCListUnstable()
         return [
@@ -42,22 +44,22 @@ def defJointGroups (self):
 
     def startABSTIMP (self):
         ### not used on hrpsys
-        self.el_svc.setServoErrorLimit("motor_joint",   sys.float_info.max)
-        self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
+        self.setServoErrorLimit("motor_joint",   sys.float_info.max)
+        self.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
+        self.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
+        self.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
+        self.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
         ###
         self.startAutoBalancer()
         # Suppress limit over message and behave like real robot that always angle-vector is in seq.
         # Latter four 0.0 are for hands.
-        self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0)
+        self.setJointAnglesRadian(self.resetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0)
         self.ic_svc.startImpedanceController("larm")
         self.ic_svc.startImpedanceController("rarm")
         self.startStabilizer()
 
 if __name__ == '__main__':
-    hcf = JAXON_RED_HrpsysConfigurator("JAXON_RED")
+    hcf = JAXON_REDCnoidHrpsysConfigurator()
     [sys.argv, connect_constraint_force_logger_ports] = hcf.parse_arg_for_connect_ports(sys.argv)
     if len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2], connect_constraint_force_logger_ports=connect_constraint_force_logger_ports)

hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py

@@ -1,8 +1,10 @@
 #!/usr/bin/env python
 
-from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import *
+import sys
+from hrpsys_choreonoid_tutorials.choreonoid_hrpsys_config import ChoreonoidHrpsysConfiguratorOrg
+from hrpsys_ros_bridge_tutorials.jaxon_red_hrpsys_config import JAXON_REDHrpsysConfigurator
 
-class JAXON_RED_HrpsysConfigurator(ChoreonoidHrpsysConfiguratorOrg):
+class JAXON_REDCnoidHrpsysConfigurator(ChoreonoidHrpsysConfiguratorOrg, JAXON_REDHrpsysConfigurator):
     def getRTCList (self):
         ##return self.getRTCListUnstable()
         return [
@@ -42,22 +44,22 @@ def defJointGroups (self):
 
     def startABSTIMP (self):
         ### not used on hrpsys
-        self.el_svc.setServoErrorLimit("motor_joint",   sys.float_info.max)
-        self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
-        self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
+        self.setServoErrorLimit("motor_joint",   sys.float_info.max)
+        self.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max)
+        self.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max)
+        self.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)
+        self.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max)
         ###
         self.startAutoBalancer()
         # Suppress limit over message and behave like real robot that always angle-vector is in seq.
         # Latter four 0.0 are for hands.
-        self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0)
+        self.setJointAnglesRadian(self.resetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0)
         self.ic_svc.startImpedanceController("larm")
         self.ic_svc.startImpedanceController("rarm")
         self.startStabilizer()
 
 if __name__ == '__main__':
-    hcf = JAXON_RED_HrpsysConfigurator("JAXON_RED")
+    hcf = JAXON_REDCnoidHrpsysConfigurator()
     [sys.argv, connect_constraint_force_logger_ports] = hcf.parse_arg_for_connect_ports(sys.argv)
     if len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2], connect_constraint_force_logger_ports=connect_constraint_force_logger_ports)

hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py

@@ -3,6 +3,12 @@
 from hrpsys_ros_bridge_tutorials.urata_hrpsys_config import *
 
 class ChoreonoidHrpsysConfiguratorOrg(URATAHrpsysConfigurator):
+    """
+    Subclass to specify Choreonoid-dependent code.
+    Please inherit this class and hrpsys configurator for each robot to specify
+    robot-dependent class for Choreonoid.
+    """
+
     def getRTCList (self):
         ##return self.getRTCListUnstable()
         return [
@@ -27,13 +33,9 @@ def getRTCList (self):
             ]
     def setupLogger(self, maxlen=15000):
         HrpsysConfigurator.setupLogger(self, maxlen)
-        self.connectLoggerPort(self.rh, 'WAIST') ##
-        self.connectLoggerPort(self.abc, 'rfsensor')
-        self.connectLoggerPort(self.abc, 'lfsensor')
+        # self.connectLoggerPort(self.rh, 'WAIST') # For debug
         for pn in filter (lambda x : re.match("Trans_", x), self.rh.ports.keys()):
             self.connectLoggerPort(self.rh, pn)
-        ##self.connectLoggerPort(self.abc, 'rhsensor')
-        ##self.connectLoggerPort(self.abc, 'lhsensor')
 
     def connectConstraintForceLoggerPorts(self):
         for pn in filter (lambda x : re.match("T_", x), self.rh.ports.keys()):
@@ -56,6 +58,12 @@ def parse_arg_for_connect_ports (self, arg_list):
             return [arg_list, False]
 
 class ChoreonoidHrpsysConfigurator(ChoreonoidHrpsysConfiguratorOrg):
+    """
+    Subclass to specify Choreonoid-dependent code with RobotHardware.
+    Please inherit this class and hrpsys configurator for each robot to specify
+    robot-dependent class for Choreonoid.
+    """
+
     def waitForRobotHardware(self, robotname="Robot"):
         '''!@brief
         Wait for RobotHardware is exists and activated.

hrpsys_ros_bridge_jvrc/scripts/jaxon_jvrc_hrpsys_config.py

@@ -1,8 +1,9 @@
 #!/usr/bin/env python
 
-from hrpsys_ros_bridge_tutorials.urata_hrpsys_config import *
+import sys
+from hrpsys_ros_bridge_tutorials.jaxon_red_hrpsys_config import JAXON_REDHrpsysConfigurator
 
-class JAXON_JVRC_HrpsysConfigurator(URATAHrpsysConfigurator):
+class JAXON_JVRC_HrpsysConfigurator(JAXON_REDHrpsysConfigurator):
     def getRTCList (self):
         ##return self.getRTCListUnstable()
         return [
@@ -45,7 +46,7 @@ def startABSTIMP (self):
         self.startStabilizer()
 
 if __name__ == '__main__':
-    hcf = JAXON_JVRC_HrpsysConfigurator("JAXON_RED")
+    hcf = JAXON_JVRC_HrpsysConfigurator()
     if len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2])
         hcf.startABSTIMP()