From a1b7ab650045d9cf0aac0afe38feb2bc94ffd06e Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 23 Jan 2018 00:38:03 +0900 Subject: [PATCH 01/14] [hrpsys_ros_bridge_tutorials] update abc default zmp offsets of jaxon blue --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index a204bf35..88e35966 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -308,7 +308,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] - abcp.default_zmp_offsets=[[0.05, 0.0, 0.0], [0.05, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; + abcp.default_zmp_offsets=[[0.05, 0.02, 0.0], [0.05, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; abcp.move_base_gain=0.8 self.abc_svc.setAutoBalancerParam(abcp) # kf setting From 0be504ba7338ea463c02850534604801e3cbaf37 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 23 Jan 2018 00:39:24 +0900 Subject: [PATCH 02/14] [hrpsys_ros_bridge_tutorials] update kf sensor rpy offset of jaxon blue --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 1 + 1 file changed, 1 insertion(+) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 88e35966..e3818639 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -314,6 +314,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] kfp.R_angle=1000 + kfp.sensorRPY_offset=[0.7*3.14/180, 0, 0] self.kf_svc.setKalmanFilterParam(kfp) # st setting stp=self.st_svc.getParameter() From 2693bc5abe3c3e2d9838252426b1f32e852c06a2 Mon Sep 17 00:00:00 2001 From: Hiroto Suzuki Date: Mon, 31 Oct 2016 09:41:04 +0900 Subject: [PATCH 03/14] [hrpsys_ros_bridge_tutorials/model/CHIDORI.PDgains*] add torque gains --- .../models/CHIDORI.PDgains.sav | 24 +++++++++---------- .../models/CHIDORI.PDgains_sim.dat | 24 +++++++++---------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav index e6b58a4c..88d43b97 100644 --- a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav +++ b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav @@ -1,12 +1,12 @@ -33000 0 240 -83000 0 240 -33000 0 240 -33000 0 240 -47000 0 240 -33000 0 240 -33000 0 240 -83000 0 240 -33000 0 240 -33000 0 240 -47000 0 240 -33000 0 240 +33000 0 240 0 1000 0 0 +83000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +47000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +83000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 +47000 0 240 0 1000 0 0 +33000 0 240 0 1000 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat index 51dceecb..4c65d587 100644 --- a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat +++ b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat @@ -1,12 +1,12 @@ -33000 240 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 -33000 240 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 +33000 240 1000 0 +83000 240 1000 0 +33000 240 1000 0 +33000 240 1000 0 +47000 240 1000 0 +33000 240 1000 0 +33000 240 1000 0 +83000 240 1000 0 +33000 240 1000 0 +33000 240 1000 0 +47000 240 1000 0 +33000 240 1000 0 From 86eba5e2babd2e2fd04e87c523132dd55445b532 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Sat, 27 Jan 2018 02:36:10 +0900 Subject: [PATCH 04/14] [hrpsys_ros_bridge_tutorials] add torque gain to servo gain file of JAXON_BLUE and CHIDORI for choreonoid simulation and real machine --- .../models/CHIDORI.PDgains.sav | 24 +-- .../models/CHIDORI.PDgains_sim.dat | 24 +-- .../models/JAXON_BLUE.PDgains.sav | 62 +++--- .../models/JAXON_BLUE.PDgains_sim.dat | 62 +++--- .../models/PDgains.sav | 200 +++++++++--------- 5 files changed, 186 insertions(+), 186 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav index 88d43b97..71200122 100644 --- a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav +++ b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains.sav @@ -1,12 +1,12 @@ -33000 0 240 0 1000 0 0 -83000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -47000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -83000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 -47000 0 240 0 1000 0 0 -33000 0 240 0 1000 0 0 +33000 0 240 1 0 0 +83000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +47000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +83000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +47000 0 240 1 0 0 +33000 0 240 1 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat index 4c65d587..951ea4ef 100644 --- a/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat +++ b/hrpsys_ros_bridge_tutorials/models/CHIDORI.PDgains_sim.dat @@ -1,12 +1,12 @@ -33000 240 1000 0 -83000 240 1000 0 -33000 240 1000 0 -33000 240 1000 0 -47000 240 1000 0 -33000 240 1000 0 -33000 240 1000 0 -83000 240 1000 0 -33000 240 1000 0 -33000 240 1000 0 -47000 240 1000 0 -33000 240 1000 0 +33000 240 1 0 +83000 240 1 0 +33000 240 1 0 +33000 240 1 0 +47000 240 1 0 +33000 240 1 0 +33000 240 1 0 +83000 240 1 0 +33000 240 1 0 +33000 240 1 0 +47000 240 1 0 +33000 240 1 0 diff --git a/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains.sav b/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains.sav index bdddf4be..b9ca6bfd 100644 --- a/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains.sav +++ b/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains.sav @@ -1,31 +1,31 @@ -33000 0 240 -83000 0 240 -33000 0 240 -33000 0 240 -47000 0 240 -33000 0 240 -33000 0 240 -83000 0 240 -33000 0 240 -33000 0 240 -47000 0 240 -33000 0 240 -83000 0 240 -83000 0 240 -83000 0 240 -10000 0 200 -10000 0 200 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 -20000 0 320 +33000 0 240 1 0 0 +83000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +47000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +83000 0 240 1 0 0 +33000 0 240 1 0 0 +33000 0 240 1 0 0 +47000 0 240 1 0 0 +33000 0 240 1 0 0 +83000 0 240 1 0 0 +83000 0 240 1 0 0 +83000 0 240 1 0 0 +10000 0 200 1 0 0 +10000 0 200 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 +20000 0 320 1 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains_sim.dat index bf9ae5da..859df320 100644 --- a/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains_sim.dat +++ b/hrpsys_ros_bridge_tutorials/models/JAXON_BLUE.PDgains_sim.dat @@ -1,31 +1,31 @@ -33000 240 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 -33000 240 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 -83000 240 -83000 240 -83000 240 -10000 200 -10000 200 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 -20000 320 \ No newline at end of file +33000 240 1 0 +83000 240 1 0 +33000 240 1 0 +33000 240 1 0 +47000 240 1 0 +33000 240 1 0 +33000 240 1 0 +83000 240 1 0 +33000 240 1 0 +33000 240 1 0 +47000 240 1 0 +33000 240 1 0 +83000 240 1 0 +83000 240 1 0 +83000 240 1 0 +10000 200 1 0 +10000 200 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 +20000 320 1 0 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/PDgains.sav b/hrpsys_ros_bridge_tutorials/models/PDgains.sav index f6577b13..0a354039 100644 --- a/hrpsys_ros_bridge_tutorials/models/PDgains.sav +++ b/hrpsys_ros_bridge_tutorials/models/PDgains.sav @@ -1,100 +1,100 @@ -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 -1 0 1 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 From 648fbaf1548d7754f6a1b17990d7845a31ea3717 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 19 Feb 2018 23:53:09 +0900 Subject: [PATCH 05/14] [hrpsys_ros_bridge_tutorials] add setting for support and landing phase --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index e3818639..85824d25 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -374,6 +374,12 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): stp.eefm_k1=[-1.36334,-1.36334] stp.eefm_k2=[-0.343983,-0.343983] stp.eefm_k3=[-0.161465,-0.161465] + stp.swing2landing_transition_time = 0.05 + stp.landing_phase_time = 0.1 + stp.landing2support_transition_time = 0.5 + leg_gains = {"support_pgain":[5,30,10,5,0.5,0.1], "support_dgain":[70,70,50,10,1,3], "landing_pgain":[5,30,10,1,0.5,0.1], "landing_dgain":[70,70,50,10,1,3]} + arm_gains = {"support_pgain":[100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100]} + stp.joint_servo_control_parameters = map (lambda x : OpenHRP.StabilizerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) self.st_svc.setParameter(stp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] From 5b15c01032bb5bdf588167609a56da92783fca6b Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 13 Nov 2018 23:01:48 +0900 Subject: [PATCH 06/14] [hrpsys_ros_bridge_tutorials] set end effectors height of jaxon3 to wacoh sensors foot --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 2 +- hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 447fd04c..f106e386 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -406,7 +406,7 @@ compile_rbrain_model_for_closed_robots(JAXON_BLUE JAXON_BLUE JAXON_BLUE ## KAWADA foot --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708," ## KAWADA foot with rubber shoes - --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," + --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.124,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.124,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," ## KAWADA foot with rubber shoes + hand contact end-coords --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot with rubber shoes + hand contact end-coords" ## JSK foot : mechanical 103.5mm, sole rubber 2mm diff --git a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml index 437d75bd..95dae5a7 100644 --- a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml +++ b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml @@ -69,10 +69,10 @@ larm-end-coords: ## KAWADA FOOT : CAD -> -0.096; gomugutsu+midori -> -0.004 rleg-end-coords: parent: RLEG_LINK5 - translate : [0, 0, -0.100] + translate : [0, 0, -0.124] lleg-end-coords: parent: LLEG_LINK5 - translate : [0, 0, -0.100] + translate : [0, 0, -0.124] ## JSK FOOT # rleg-end-coords: # parent: RLEG_LINK5 From e1f9817f7f102955935f7031dd94cb4f0fb85d17 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 13 Nov 2018 23:03:44 +0900 Subject: [PATCH 07/14] [hrpsys_ros_bridge_tutorials] update st LIP feedback gain JAXON3 for leg + body model --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 85824d25..0bcf9c9a 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -370,10 +370,10 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): larm_vertices = lleg_vertices stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) stp.eefm_cogvel_cutoff_freq = 4.0 - # for only leg - stp.eefm_k1=[-1.36334,-1.36334] - stp.eefm_k2=[-0.343983,-0.343983] - stp.eefm_k3=[-0.161465,-0.161465] + # for leg + body + stp.eefm_k1 = [-1.40928, -1.40928] + stp.eefm_k2 = [-0.398194, -0.398194] + stp.eefm_k3 = [-0.178433, -0.178433] stp.swing2landing_transition_time = 0.05 stp.landing_phase_time = 0.1 stp.landing2support_transition_time = 0.5 From eb407b546fc65dd60fe87f60d5a3fbf20c8444fc Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 13 Nov 2018 23:06:08 +0900 Subject: [PATCH 08/14] [hrpsys_ros_bridge_tutorials] debug function of reset landing pose for JAXON3 in hrpsys python script --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 0bcf9c9a..d3aa42a4 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -750,7 +750,7 @@ def setCollisionFreeResetPose(self): self.seq_svc.setJointAngles(self.jaxonCollisionFreeResetPose(), 10.0) def setResetLandingPose(self): - if self.ROBOT_NAME == 0: + if self.ROBOT_NAME == "JAXON_BLUE": self.seq_svc.setJointAngles(self.jaxonBlueResetLandingPose(), 5.0) if self.ROBOT_NAME.find("JAXON") == 0: self.seq_svc.setJointAngles(self.jaxonResetLandingPose(), 5.0) From c39798f3f11511f097a17cfd1714a37bf2294ff9 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 13 Nov 2018 23:11:33 +0900 Subject: [PATCH 09/14] [hrpsys_ros_bridge_tutorials] set reset landing pose of JAXON3 --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 2 +- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index f106e386..89383810 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -219,7 +219,7 @@ compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W --simulation-timestep-option "0.004" --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708" ) -gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) +# gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G --conf-file-option "end_effectors: rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708" --conf-file-option "# for ThermoEstimator" diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index d3aa42a4..c1902d99 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -693,7 +693,7 @@ def jaxonResetLandingPose (self): return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] def jaxonBlueResetLandingPose (self): - return self.jaxonBlueResetPose() + return [0.0,0.0,-0.73796,1.45379,-0.715828,0.0, 0.0,0.0,-0.73796,1.45379,-0.715828,0.0, 0.0,0.0,0.0, 0.0,0.0, 0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,0.0, 0.698132,0.349066,0.087266,-1.39626,0.0,0.0,0.0] # handmade def chidoriResetLandingPose (self): From 964e437746338248c3f52a55fff4f2ed3c5ac973 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Tue, 13 Nov 2018 23:28:25 +0900 Subject: [PATCH 10/14] [hrpsys_ros_bridge_tutorials] set default joint control mode TORQUE in JAXON_BLUE --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index c1902d99..81a7e820 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -374,6 +374,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): stp.eefm_k1 = [-1.40928, -1.40928] stp.eefm_k2 = [-0.398194, -0.398194] stp.eefm_k3 = [-0.178433, -0.178433] + stp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE stp.swing2landing_transition_time = 0.05 stp.landing_phase_time = 0.1 stp.landing2support_transition_time = 0.5 @@ -381,6 +382,8 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): arm_gains = {"support_pgain":[100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100]} stp.joint_servo_control_parameters = map (lambda x : OpenHRP.StabilizerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) self.st_svc.setParameter(stp) + # rh setting + self.rh_svc.setJointControlMode("all",OpenHRP.RobotHardwareService.TORQUE) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] #gg.stride_parameter=[0.1,0.05,10.0] From 31425afc5baac3307910397402eb09f3f6a463d4 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Fri, 30 Nov 2018 22:02:57 +0900 Subject: [PATCH 11/14] [hrpsys_ros_bridge_tutorials] add end-coords settings for footstep planning of JAXON BLUE --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 89383810..03cfc4c0 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -732,6 +732,10 @@ if(${jsk_models_FOUND}) attach_sensor_and_endeffector_to_staro_urdf(CHIDORI CHIDORI.urdf CHIDORI_SENSORS.urdf chidori.yaml) endif() +if(${jsk_models_FOUND}) + attach_sensor_and_endeffector_to_staro_urdf(JAXON_BLUE JAXON_BLUE.urdf JAXON_BLUE_SENSORS.urdf jaxon_blue.yaml) +endif() + # for SampleRobot attach_sensor_and_endeffector_to_hrp2jsk_urdf(SampleRobot SampleRobot.urdf SampleRobot_WH_SENSORS.urdf samplerobot.yaml) From 6329f02e83f589124b2101278dee700580185898 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Fri, 21 Dec 2018 23:23:15 +0900 Subject: [PATCH 12/14] (jaxon blue local diff) --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 03cfc4c0..a0e3412f 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -411,7 +411,8 @@ compile_rbrain_model_for_closed_robots(JAXON_BLUE JAXON_BLUE JAXON_BLUE --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot with rubber shoes + hand contact end-coords" ## JSK foot : mechanical 103.5mm, sole rubber 2mm --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708," - --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2" + # --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2" + --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5" --conf-file-option "collision_model: convex hull" --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)" --conf-file-option "seq_optional_data_dim: 8" From a41ad779a6aa04ec63cb2c241999c6326759c220 Mon Sep 17 00:00:00 2001 From: Takuma-Hiraoka Date: Tue, 25 Apr 2023 21:28:04 +0900 Subject: [PATCH 13/14] [hrpsys_ros_bridge_tutorials] update end-coords --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 4 ++-- hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml | 4 ++-- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index d888f77f..00a26e7a 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -223,7 +223,7 @@ compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W --simulation-timestep-option "0.004" --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708" ) -# gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) +gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G --conf-file-option "end_effectors: rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708" --conf-file-option "# for ThermoEstimator" @@ -402,7 +402,7 @@ compile_rbrain_model_for_closed_robots(JAXON_BLUE JAXON_BLUE JAXON_BLUE ## KAWADA foot --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708," ## KAWADA foot with rubber shoes - --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.124,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.124,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," + --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.05,0.0,-0.124,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.05,0.0,-0.124,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," ## KAWADA foot with rubber shoes + hand contact end-coords --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot with rubber shoes + hand contact end-coords" ## JSK foot : mechanical 103.5mm, sole rubber 2mm diff --git a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml index 95dae5a7..7c0af058 100644 --- a/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml +++ b/hrpsys_ros_bridge_tutorials/models/jaxon_blue.yaml @@ -69,10 +69,10 @@ larm-end-coords: ## KAWADA FOOT : CAD -> -0.096; gomugutsu+midori -> -0.004 rleg-end-coords: parent: RLEG_LINK5 - translate : [0, 0, -0.124] + translate : [0.05, 0, -0.124] lleg-end-coords: parent: LLEG_LINK5 - translate : [0, 0, -0.124] + translate : [0.05, 0, -0.124] ## JSK FOOT # rleg-end-coords: # parent: RLEG_LINK5 diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 5de74fec..5f8327a4 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -326,7 +326,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] - abcp.default_zmp_offsets=[[0.05, 0.02, 0.0], [0.05, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; + abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; abcp.move_base_gain=0.8 self.abc_svc.setAutoBalancerParam(abcp) # kf setting From fcfe33d5ad24ad5d0862c4bc50861d3117ddbad2 Mon Sep 17 00:00:00 2001 From: Takuma-Hiraoka Date: Wed, 26 Apr 2023 11:02:36 +0900 Subject: [PATCH 14/14] remove parameters for hrpsys master --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 5f8327a4..1cb5f049 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py @@ -392,16 +392,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): stp.eefm_k1 = [-1.40928, -1.40928] stp.eefm_k2 = [-0.398194, -0.398194] stp.eefm_k3 = [-0.178433, -0.178433] - stp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE - stp.swing2landing_transition_time = 0.05 - stp.landing_phase_time = 0.1 - stp.landing2support_transition_time = 0.5 - leg_gains = {"support_pgain":[5,30,10,5,0.5,0.1], "support_dgain":[70,70,50,10,1,3], "landing_pgain":[5,30,10,1,0.5,0.1], "landing_dgain":[70,70,50,10,1,3]} - arm_gains = {"support_pgain":[100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100]} - stp.joint_servo_control_parameters = map (lambda x : OpenHRP.StabilizerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) self.st_svc.setParameter(stp) - # rh setting - self.rh_svc.setJointControlMode("all",OpenHRP.RobotHardwareService.TORQUE) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] #gg.stride_parameter=[0.1,0.05,10.0]