From 28376eedbf5c2460a96056391360944211ddd132 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 13 Nov 2017 14:14:08 +0900 Subject: [PATCH 1/4] [hrpsys_ros_bridge_tutorials] modify JAXON3's ST damping gain and swing leg modification gain --- .../urata_hrpsys_config.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 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 91465817..0ba49a95 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 @@ -327,24 +327,24 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): #stp.eefm_body_attitude_control_gain=[0, 0.5] stp.eefm_body_attitude_control_gain=[0.5, 0.5] stp.eefm_body_attitude_control_time_const=[1000, 1000] - stp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5, 20*1.6*1.1*1.5, 1e5], - [20*1.6*1.1*1.5, 20*1.6*1.1*1.5, 1e5], - [20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5], - [20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5]] - stp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5], - [3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5], - [3500*1.6*6*0.8, 3500*1.6*6*0.8, 3500*1.6*1.1*1.5*0.8], - [3500*1.6*6*0.8, 3500*1.6*6*0.8, 3500*1.6*1.1*1.5*0.8]] - stp.eefm_swing_pos_damping_gain = stp.eefm_pos_damping_gain[0] - stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] + stp.eefm_rot_damping_gain = [[25, 25, 1e5], # modification with kojio + [25, 25, 1e5], + [63.36, 63.36, 1e5], + [63.36, 63.36, 1e5]] + stp.eefm_pos_damping_gain = [[33600.0, 33600.0, 3234.0], # modification with kojio xy=10000? + [33600.0, 33600.0, 3234.0], + [26880.0, 26880.0, 7392.0], + [26880.0, 26880.0, 7392.0]] + stp.eefm_swing_pos_damping_gain = stp.eefm_pos_damping_gain[0] # same with support leg + stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] # same with support leg stp.eefm_rot_compensation_limit = [math.radians(10), math.radians(10), math.radians(10), math.radians(10)] stp.eefm_pos_compensation_limit = [0.025, 0.025, 0.050, 0.050] stp.eefm_swing_damping_force_thre=[200]*3 stp.eefm_swing_damping_moment_thre=[15]*3 stp.eefm_use_swing_damping=True stp.eefm_ee_error_cutoff_freq=20.0 - stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*4 - stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*4 + # stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*4 + # stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*4 stp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] stp.eefm_rot_time_const = [[1.5/1.1, 1.5/1.1, 1.5/1.1]]*4 stp.eefm_pos_time_const_support = [[3.0/1.1, 3.0/1.1, 1.5/1.1]]*4 From bd3bdabf02c8225b0758d7a5098fb45fe09f46df Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 13 Nov 2017 14:19:51 +0900 Subject: [PATCH 2/4] [hrpsys_ros_bridge_tutorials] modify JAXON3's ST state feedback gain --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 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 0ba49a95..4d658e7a 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 @@ -369,9 +369,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 - stp.eefm_k1=[-1.48412,-1.48412] - stp.eefm_k2=[-0.486727,-0.486727] - stp.eefm_k3=[-0.198033,-0.198033] + # for only leg + stp.eefm_k1=[-1.36334,-1.36334] + stp.eefm_k2=[-0.343983,-0.343983] + stp.eefm_k3=[-0.161465,-0.161465] self.st_svc.setParameter(stp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] From 5504cab533773c34b5d42d7935f8da0b779c526f Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 13 Nov 2017 14:21:23 +0900 Subject: [PATCH 3/4] [hrpsys_ros_bridge_tutorials] modify JAXON3's ABC default zmp offset --- .../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 4d658e7a..a694bec0 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.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; + abcp.default_zmp_offsets=[[0.05, 0.0, 0.0], [0.05, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; abcp.move_base_gain=0.8 self.abc_svc.setAutoBalancerParam(abcp) # kf setting From a9fca9e3bcd470fe1bf2c90c6d34ca6e24be8a3e Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 13 Nov 2017 14:22:25 +0900 Subject: [PATCH 4/4] [hrpsys_ros_bridge_tutorials] modify JAXON3's conf code miner change --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 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 a694bec0..a204bf35 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 @@ -320,7 +320,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): #stp.st_algorithm=OpenHRP.StabilizerService.EEFM #stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP - stp.emergency_check_mode=OpenHRP.StabilizerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 + stp.emergency_check_mode=OpenHRP.StabilizerService.CP stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] @@ -355,8 +355,8 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): # foot margin param stp.eefm_leg_inside_margin=0.05 stp.eefm_leg_outside_margin=0.05 - stp.eefm_leg_front_margin=0.12 - stp.eefm_leg_rear_margin=0.09 + stp.eefm_leg_front_margin=0.16 + stp.eefm_leg_rear_margin=0.06 rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_inside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_outside_margin]), OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_outside_margin]),