From e8a67a7822a8194697b7e70fee05e87fb4b30dfa Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Mon, 24 Dec 2018 21:18:53 +0900 Subject: [PATCH 1/3] Modify setup.py for each robot to deal with urata_hrpsys_config.py --- .../scripts/chidori_rh_setup.py | 21 ++++++----- .../scripts/jaxon_blue_rh_setup.py | 36 ++++++++----------- .../scripts/jaxon_red_rh_setup.py | 20 ++++++----- .../scripts/jaxon_red_setup.py | 20 ++++++----- .../choreonoid_hrpsys_config.py | 6 +--- .../scripts/jaxon_jvrc_hrpsys_config.py | 7 ++-- 6 files changed, 51 insertions(+), 59 deletions(-) diff --git a/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py index d075287b..a0318966 100755 --- a/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py +++ b/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,11 +32,6 @@ 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") @@ -40,7 +39,7 @@ def startABSTIMP (self): 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) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py index a632c136..47d34315 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py +++ b/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) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index 4538ed10..72ebb818 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/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.setJointAngles(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) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index 366b38af..7815ab9c 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/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.setJointAngles(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) diff --git a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py index f75f9b83..4d4c214a 100755 --- a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py +++ b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py @@ -27,13 +27,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()): diff --git a/hrpsys_ros_bridge_jvrc/scripts/jaxon_jvrc_hrpsys_config.py b/hrpsys_ros_bridge_jvrc/scripts/jaxon_jvrc_hrpsys_config.py index d2e58077..07f25071 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/jaxon_jvrc_hrpsys_config.py +++ b/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() From 1fbe88b74f760a6ce23af72a48dde59353693dc7 Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Fri, 18 Jan 2019 21:18:07 +0900 Subject: [PATCH 2/3] [choreonoid_hrpsys_config.py] Add docstring to describe ChoreonoidHrpsysConfigurator --- .../choreonoid_hrpsys_config.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py index 4d4c214a..fff753ba 100755 --- a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py +++ b/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 [ @@ -52,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. From c0c110b961a300ffda707677dc8e70aac9cbd025 Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Fri, 19 Jul 2019 20:31:16 +0900 Subject: [PATCH 3/3] [hrpsys_choreonoid_tutorials/scripts] Fix bug of the unit of joint angles --- hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py | 2 +- hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index 72ebb818..b72b3860 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py @@ -53,7 +53,7 @@ def startABSTIMP (self): 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.setJointAngles(self.resetPose()+[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() diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index 7815ab9c..2a174ff5 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py @@ -53,7 +53,7 @@ def startABSTIMP (self): 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.setJointAngles(self.resetPose()+[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()