From ab2048a768995ff7788529d89b3eb1f860e19064 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 27 Apr 2018 21:29:25 +0900 Subject: [PATCH 01/83] add param for auto-stabilizer --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 2 +- .../urata_hrpsys_config.py | 79 +++++++++++++++++++ 2 files changed, 80 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 0d5bc5f9..512efa28 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 a204bf35..4060ef67 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 @@ -268,6 +268,85 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_k2=[-0.486727,-0.486727] stp.eefm_k3=[-0.198033,-0.198033] self.st_svc.setParameter(stp) + # AutoSt setting + stp=self.abc_svc.getStabilizerParam() + #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM + #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 + stp.cp_check_margin=[0.05, 0.045, 0, 0.095] + stp.k_brot_p=[0, 0] + stp.k_brot_tc=[1000, 1000] + #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] + if self.ROBOT_NAME == "JAXON": + stp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5*1.2*1.65*1.1, 20*1.6*1.1*1.5*1.2*1.65*1.1, 1e5]]*4 + stp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5*1.2*1.1]]*4 + stp.eefm_swing_rot_damping_gain=[20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5] + stp.eefm_swing_pos_damping_gain=[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.4] + stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] + stp.eefm_pos_compensation_limit = [0.06, 0.06, 0.050, 0.050] + elif self.ROBOT_NAME == "JAXON_RED": + 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_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] + stp.eefm_pos_compensation_limit = [0.08, 0.08, 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_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 + stp.eefm_wrench_alpha_blending=0.7 + stp.eefm_pos_time_const_swing=0.06 + stp.eefm_pos_transition_time=0.01 + stp.eefm_pos_margin_time=0.02 + # foot margin param + if foot == "KAWADA": + ## KAWADA foot : mechanical param is => inside 0.055, front 0.13, rear 0.1 + 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 + elif foot == "JSK": + ## JSK foot : mechanical param is -> inside 0.075, front 0.11, rear 0.11 + stp.eefm_leg_inside_margin=0.07 + stp.eefm_leg_outside_margin=0.07 + stp.eefm_leg_front_margin=0.1 + stp.eefm_leg_rear_margin=0.1 + elif foot == "LEPTRINO": + stp.eefm_leg_inside_margin=0.05 + stp.eefm_leg_outside_margin=0.05 + stp.eefm_leg_front_margin=0.115 + stp.eefm_leg_rear_margin=0.115 + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] + rarm_vertices = rleg_vertices + larm_vertices = lleg_vertices + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.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] + self.abc_svc.setStabilizerParam(stp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] #gg.stride_parameter=[0.1,0.05,10.0] From 248616a42d8ff112987ba3770e0088ef1a10f47b Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 25 May 2018 15:23:53 +0900 Subject: [PATCH 02/83] add chidori local-diff --- .../urata_hrpsys_config.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 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 4060ef67..1532f919 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 @@ -562,7 +562,7 @@ def setStAbcParametersCHIDORI (self): stp.k_brot_tc=[1000, 1000] 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*0.5, 20*1.6*1.1*1.5*0.5, 1e5]]*2 + stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 @@ -578,6 +578,7 @@ def setStAbcParametersCHIDORI (self): stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 + stp.use_zmp_truncation=True # foot margin param ## KAWADA foot # mechanical param is => inside 0.055, front 0.13, rear 0.1 @@ -585,10 +586,10 @@ def setStAbcParametersCHIDORI (self): # #stp.eefm_leg_inside_margin=0.04 # stp.eefm_leg_front_margin=0.12 # stp.eefm_leg_rear_margin=0.09 - tmp_leg_inside_margin=0.05 - tmp_leg_outside_margin=0.05 - tmp_leg_front_margin=0.12 - tmp_leg_rear_margin=0.09 + tmp_leg_inside_margin=0.065 + tmp_leg_outside_margin=0.065 + tmp_leg_front_margin=0.13 + tmp_leg_rear_margin=0.1 # JSK foot # # mechanical param is -> inside 0.075, front 0.11, rear 0.11 # stp.eefm_leg_inside_margin=0.07 From e8188066f750997a253c0762148879c4f4abd77b Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 25 May 2018 15:49:16 +0900 Subject: [PATCH 03/83] add chidori autost param --- .../urata_hrpsys_config.py | 80 ++++++++++++++++++- 1 file changed, 78 insertions(+), 2 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 1532f919..6676243e 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 @@ -271,8 +271,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # AutoSt setting stp=self.abc_svc.getStabilizerParam() #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] @@ -296,10 +296,20 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): [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_rot_damping_gain = [[1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], + # [1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], + # [1e5*20*1.6*1.1*1.5*1.2, 1e5*20*1.6*1.1*1.5*1.2, 1e5], + # [1e5*20*1.6*1.1*1.5*1.2, 1e5*20*1.6*1.1*1.5*1.2, 1e5]] + # stp.eefm_pos_damping_gain = [[1e5*3500*1.6*6, 1e5*3500*1.6*6, 1e5*3500*1.6*1.1*1.5], + # [1e5*3500*1.6*6, 1e5*3500*1.6*6, 1e5*3500*1.6*1.1*1.5], + # [1e5*3500*1.6*6*0.8, 1e5*3500*1.6*6*0.8, 1e5*3500*1.6*1.1*1.5*0.8], + # [1e5*3500*1.6*6*0.8, 1e5*3500*1.6*6*0.8, 1e5*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_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] stp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] + stp.eefm_zmp_delay_time_const=[0, 0] + stp.use_zmp_truncation=True stp.eefm_swing_damping_force_thre=[200]*3 stp.eefm_swing_damping_moment_thre=[15]*3 stp.eefm_use_swing_damping=True @@ -619,6 +629,72 @@ def setStAbcParametersCHIDORI (self): stp.eefm_k2=[-0.368975,-0.368975] stp.eefm_k3=[-0.169915,-0.169915] self.st_svc.setParameter(stp) + # AutoST setting + stp=self.abc_svc.getStabilizerParam() + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.k_brot_p=[0, 0] + stp.k_brot_tc=[1000, 1000] + 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*1.5, 1e5]]*2 + stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 + stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 + stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 + stp.eefm_use_swing_damping=True + 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_compensation_limit = [math.radians(30), math.radians(30)] + stp.eefm_pos_compensation_limit = [0.05, 0.05] + stp.eefm_ee_error_cutoff_freq=20.0 + stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*2 + stp.eefm_wrench_alpha_blending=0.7 + stp.eefm_pos_time_const_swing=0.06 + stp.eefm_pos_transition_time=0.01 + stp.eefm_pos_margin_time=0.02 + stp.use_zmp_truncation=True + stp.eefm_zmp_delay_time_const=[0, 0] + # foot margin param + ## KAWADA foot + # mechanical param is => inside 0.055, front 0.13, rear 0.1 + # stp.eefm_leg_inside_margin=0.05 + # #stp.eefm_leg_inside_margin=0.04 + # stp.eefm_leg_front_margin=0.12 + # stp.eefm_leg_rear_margin=0.09 + tmp_leg_inside_margin=0.065 + tmp_leg_outside_margin=0.065 + tmp_leg_front_margin=0.13 + tmp_leg_rear_margin=0.1 + # JSK foot + # # mechanical param is -> inside 0.075, front 0.11, rear 0.11 + # stp.eefm_leg_inside_margin=0.07 + # stp.eefm_leg_front_margin=0.1 + # stp.eefm_leg_rear_margin=0.1 + stp.eefm_leg_inside_margin=tmp_leg_inside_margin + stp.eefm_leg_outside_margin=tmp_leg_outside_margin + stp.eefm_leg_front_margin=tmp_leg_front_margin + stp.eefm_leg_rear_margin=tmp_leg_rear_margin + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + # stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_cogvel_cutoff_freq = 4.0 + stp.eefm_k1=[-1.48412,-1.48412] + + # stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_cogvel_cutoff_freq = 4.0 + # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* + stp.eefm_k1=[-1.38444,-1.38444] + stp.eefm_k2=[-0.368975,-0.368975] + stp.eefm_k3=[-0.169915,-0.169915] + self.abc_svc.setStabilizerParam(stp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] #gg.stride_parameter=[0.1,0.05,10.0] From 1ddeba5336639eb734e54a8234a930fcf33b14d0 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 30 May 2018 16:21:01 +0900 Subject: [PATCH 04/83] modify move_base_gain --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 6676243e..665b6d12 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 @@ -183,7 +183,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; elif self.ROBOT_NAME == "JAXON_RED": abcp.default_zmp_offsets=[[0.0, 0.01, 0.0], [0.0, -0.01, 0.0], [0, 0, 0], [0, 0, 0]]; - abcp.move_base_gain=0.8 + abcp.move_base_gain=1.0 self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] @@ -559,7 +559,7 @@ def setStAbcParametersCHIDORI (self): abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; - abcp.move_base_gain=0.8 + abcp.move_base_gain=1.0 self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] From ebd0af50ec5b231ae1a08e7b23b9a81637c5ce0f Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 11 Jul 2018 15:56:26 +0900 Subject: [PATCH 05/83] suport modify footstep timing --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 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 665b6d12..91e05657 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 @@ -309,6 +309,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] stp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] stp.eefm_zmp_delay_time_const=[0, 0] + stp.detection_time_to_air=1.0 stp.use_zmp_truncation=True stp.eefm_swing_damping_force_thre=[200]*3 stp.eefm_swing_damping_moment_thre=[15]*3 @@ -374,12 +375,13 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.swing_trajectory_delay_time_offset=0.15 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] gg.swing_trajectory_final_distance_weight=3.0 - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*117.338; gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; gg.optional_go_pos_finalize_footstep_num=1 + gg.overwritable_footstep_index_offset=0 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting limbs = ['rarm', 'larm'] @@ -655,6 +657,7 @@ def setStAbcParametersCHIDORI (self): stp.eefm_pos_margin_time=0.02 stp.use_zmp_truncation=True stp.eefm_zmp_delay_time_const=[0, 0] + stp.detection_time_to_air=1.0 # foot margin param ## KAWADA foot # mechanical param is => inside 0.055, front 0.13, rear 0.1 @@ -710,11 +713,12 @@ def setStAbcParametersCHIDORI (self): gg.swing_trajectory_delay_time_offset=0.2 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] gg.swing_trajectory_final_distance_weight=3.0 - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*117.338; gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; + gg.overwritable_footstep_index_offset=0 self.abc_svc.setGaitGeneratorParam(gg) def setStAbcParametersTQLEG0 (self): From 7324224d7bdfa5084df087fa88fe401c6b72c4ea Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 11 Jul 2018 16:00:34 +0900 Subject: [PATCH 06/83] chiodri diff --- .../urata_hrpsys_config.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 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 91e05657..f1a61ec7 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 @@ -560,7 +560,7 @@ def setStAbcParametersCHIDORI (self): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; - abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; + abcp.default_zmp_offsets=[[0.01, 0.01, 0.0], [0.01, -0.01, 0.0]]; # 20170704 abcp.move_base_gain=1.0 self.abc_svc.setAutoBalancerParam(abcp) # kf setting @@ -639,8 +639,10 @@ def setStAbcParametersCHIDORI (self): stp.k_brot_tc=[1000, 1000] 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*1.5, 1e5]]*2 - stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 + # stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 + # stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 + stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 # 20180711 + stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5]]*2 # 20180711 stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_use_swing_damping=True @@ -667,8 +669,8 @@ def setStAbcParametersCHIDORI (self): # stp.eefm_leg_rear_margin=0.09 tmp_leg_inside_margin=0.065 tmp_leg_outside_margin=0.065 - tmp_leg_front_margin=0.13 - tmp_leg_rear_margin=0.1 + tmp_leg_front_margin=0.09 + tmp_leg_rear_margin=0.15 # JSK foot # # mechanical param is -> inside 0.075, front 0.11, rear 0.11 # stp.eefm_leg_inside_margin=0.07 From 9e881261a1b38f5c1452a17f4f5ef83ce181ce7b Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Mon, 16 Jul 2018 18:31:21 +0900 Subject: [PATCH 07/83] truncate zmp in hrpsys --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 f1a61ec7..fabd721f 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 @@ -271,8 +271,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # AutoSt setting stp=self.abc_svc.getStabilizerParam() #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] @@ -310,7 +310,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] stp.eefm_zmp_delay_time_const=[0, 0] stp.detection_time_to_air=1.0 - stp.use_zmp_truncation=True + # stp.use_zmp_truncation=True stp.eefm_swing_damping_force_thre=[200]*3 stp.eefm_swing_damping_moment_thre=[15]*3 stp.eefm_use_swing_damping=True @@ -590,7 +590,7 @@ def setStAbcParametersCHIDORI (self): stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - stp.use_zmp_truncation=True + # stp.use_zmp_truncation=True # foot margin param ## KAWADA foot # mechanical param is => inside 0.055, front 0.13, rear 0.1 @@ -657,7 +657,7 @@ def setStAbcParametersCHIDORI (self): stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - stp.use_zmp_truncation=True + # stp.use_zmp_truncation=True stp.eefm_zmp_delay_time_const=[0, 0] stp.detection_time_to_air=1.0 # foot margin param From 830bd83c14689427e0fe21ced3149d4f0d1cac57 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Tue, 2 Oct 2018 21:42:25 +0900 Subject: [PATCH 08/83] update --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 fabd721f..d74e5151 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 @@ -271,8 +271,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # AutoSt setting stp=self.abc_svc.getStabilizerParam() #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] From 5f973639bda95f75ad52e4d2d42bd93633bd8a74 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 3 Oct 2018 22:45:12 +0900 Subject: [PATCH 09/83] update --- .../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 d74e5151..ae5c6bed 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 @@ -560,7 +560,7 @@ def setStAbcParametersCHIDORI (self): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; - abcp.default_zmp_offsets=[[0.01, 0.01, 0.0], [0.01, -0.01, 0.0]]; # 20170704 + abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0]]; # 20170704 abcp.move_base_gain=1.0 self.abc_svc.setAutoBalancerParam(abcp) # kf setting From b106576cf1fcdb29d18aae2acc52b0d45eaae1ee Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 12 Dec 2018 19:18:39 +0900 Subject: [PATCH 10/83] support leptrino --- .../urata_hrpsys_config.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 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 ae5c6bed..4a62d470 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 @@ -651,8 +651,8 @@ def setStAbcParametersCHIDORI (self): stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30)] stp.eefm_pos_compensation_limit = [0.05, 0.05] stp.eefm_ee_error_cutoff_freq=20.0 - stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*2 - stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_rot_spring_gain=[[0.0, 0.0, 0.0]]*2 + stp.eefm_swing_pos_spring_gain=[[0.0, 0.0, 0.0]]*2 stp.eefm_wrench_alpha_blending=0.7 stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 @@ -669,8 +669,8 @@ def setStAbcParametersCHIDORI (self): # stp.eefm_leg_rear_margin=0.09 tmp_leg_inside_margin=0.065 tmp_leg_outside_margin=0.065 - tmp_leg_front_margin=0.09 - tmp_leg_rear_margin=0.15 + tmp_leg_front_margin=0.15 + tmp_leg_rear_margin=0.09 # JSK foot # # mechanical param is -> inside 0.075, front 0.11, rear 0.11 # stp.eefm_leg_inside_margin=0.07 @@ -689,10 +689,6 @@ def setStAbcParametersCHIDORI (self): OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) - # stp.eefm_zmp_delay_time_const=[0.055, 0.055] - stp.eefm_cogvel_cutoff_freq = 4.0 - stp.eefm_k1=[-1.48412,-1.48412] - # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* From 8ec3a9d0c897c0f7c5f579129abe2366e061dca1 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 19 Dec 2018 16:43:13 +0900 Subject: [PATCH 11/83] update --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 4a62d470..7da3b36f 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 @@ -315,8 +315,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): 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=[[0.0, 0.0, 0.0]]*4 + stp.eefm_swing_pos_spring_gain=[[0.0, 0.0, 0.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 e4fce3b1c1a36ac6bd67565a74816a949cbd6c19 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Mon, 24 Dec 2018 15:54:09 +0900 Subject: [PATCH 12/83] update --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 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 7da3b36f..e866b014 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 @@ -561,7 +561,7 @@ def setStAbcParametersCHIDORI (self): abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0]]; # 20170704 - abcp.move_base_gain=1.0 + abcp.move_base_gain=1.2 self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] @@ -631,8 +631,14 @@ def setStAbcParametersCHIDORI (self): stp.eefm_k2=[-0.368975,-0.368975] stp.eefm_k3=[-0.169915,-0.169915] self.st_svc.setParameter(stp) - # AutoST setting + # Emergency Stopper + esp = self.es_svc.getEmergencyStopperParam()[1] + esp.default_retrieve_time = 1.0 + self.es_svc.setEmergencyStopperParam(esp) + # autost setting stp=self.abc_svc.getStabilizerParam() + stp.is_estop_while_walking=True + stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.k_brot_p=[0, 0] From 9f217d8692cda9120699ab92d65376269369d73b Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sat, 26 Jan 2019 17:26:43 +0900 Subject: [PATCH 13/83] update --- .../urata_hrpsys_config.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 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 e866b014..e7d36102 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 @@ -639,8 +639,8 @@ def setStAbcParametersCHIDORI (self): stp=self.abc_svc.getStabilizerParam() stp.is_estop_while_walking=True stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] stp.eefm_body_attitude_control_gain=[0.5, 0.5] @@ -656,9 +656,11 @@ def setStAbcParametersCHIDORI (self): stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30)] stp.eefm_pos_compensation_limit = [0.05, 0.05] - stp.eefm_ee_error_cutoff_freq=20.0 - stp.eefm_swing_rot_spring_gain=[[0.0, 0.0, 0.0]]*2 - stp.eefm_swing_pos_spring_gain=[[0.0, 0.0, 0.0]]*2 + stp.eefm_ee_error_cutoff_freq=10000 + stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*2 stp.eefm_wrench_alpha_blending=0.7 stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 From ae40da2b0de09de052a48b6bee20a50a089d9b32 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 30 Jan 2019 17:45:32 +0900 Subject: [PATCH 14/83] set swingEEmodification param --- .../urata_hrpsys_config.py | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 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 e7d36102..ffe71239 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 @@ -309,14 +309,16 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] stp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] stp.eefm_zmp_delay_time_const=[0, 0] - stp.detection_time_to_air=1.0 - # stp.use_zmp_truncation=True + stp.detection_time_to_air=5.0 + stp.use_zmp_truncation=True 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=[[0.0, 0.0, 0.0]]*4 - stp.eefm_swing_pos_spring_gain=[[0.0, 0.0, 0.0]]*4 + # stp.eefm_ee_error_cutoff_freq=10000 # not used + stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0]]*4 + stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0]]*4 + stp.eefm_swing_rot_time_const=[[10.0, 10.0, 10.0]]*4 + stp.eefm_swing_pos_time_const=[[10.0, 10.0, 10.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 @@ -639,16 +641,20 @@ def setStAbcParametersCHIDORI (self): stp=self.abc_svc.getStabilizerParam() stp.is_estop_while_walking=True stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] 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*1.5, 1e5]]*2 # stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 - stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 # 20180711 - stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5]]*2 # 20180711 + # stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 # 20180711 + # stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5]]*2 # 20180711 + stp.eefm_rot_damping_gain = [[100, 100, 1e5], + [100, 100, 1e5]] + stp.eefm_pos_damping_gain = [[16800.0, 16800.0, 7500.0], + [16800.0, 16800.0, 7500.0]] stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_use_swing_damping=True @@ -656,18 +662,18 @@ def setStAbcParametersCHIDORI (self): stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30)] stp.eefm_pos_compensation_limit = [0.05, 0.05] - stp.eefm_ee_error_cutoff_freq=10000 - stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*2 - stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*2 - stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*2 - stp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*2 + # stp.eefm_ee_error_cutoff_freq=10000 # not used + stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0]]*2 + stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0]]*2 + stp.eefm_swing_rot_time_const=[[10.0, 10.0, 10.0]]*2 + stp.eefm_swing_pos_time_const=[[10.0, 10.0, 10.0]]*2 stp.eefm_wrench_alpha_blending=0.7 stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - # stp.use_zmp_truncation=True + stp.use_zmp_truncation=True stp.eefm_zmp_delay_time_const=[0, 0] - stp.detection_time_to_air=1.0 + stp.detection_time_to_air=5.0 # foot margin param ## KAWADA foot # mechanical param is => inside 0.055, front 0.13, rear 0.1 From 844f0322b4a67112f0be46e8477394a9c344ba56 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 30 Jan 2019 17:54:13 +0900 Subject: [PATCH 15/83] change st gain param --- .../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 ffe71239..f4598371 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 @@ -706,9 +706,9 @@ def setStAbcParametersCHIDORI (self): # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* - stp.eefm_k1=[-1.38444,-1.38444] - stp.eefm_k2=[-0.368975,-0.368975] - stp.eefm_k3=[-0.169915,-0.169915] + stp.eefm_k1=[-1.3866, -1.3866] + stp.eefm_k2=[-0.371526, -0.371526] + stp.eefm_k3=[-0.170713, -0.170713] self.abc_svc.setStabilizerParam(stp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] From 563df016454993edcb01ff3a48db4ae1135451a1 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 31 Jan 2019 15:07:11 +0900 Subject: [PATCH 16/83] update --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 f4598371..14a05fc0 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 @@ -271,8 +271,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # AutoSt setting stp=self.abc_svc.getStabilizerParam() #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] @@ -640,9 +640,9 @@ def setStAbcParametersCHIDORI (self): # autost setting stp=self.abc_svc.getStabilizerParam() stp.is_estop_while_walking=True - stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] stp.eefm_body_attitude_control_gain=[0.5, 0.5] From 004e61434b5c8d028d65bcdbc7db3e4a6cede0fc Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 31 Jan 2019 19:09:02 +0900 Subject: [PATCH 17/83] rm waist collision pair --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 512efa28..92594c02 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -408,7 +408,7 @@ compile_rbrain_model_for_closed_robots(CHIDORI CHIDORI CHIDORI --conf-dt-option "0.002" --simulation-timestep-option "0.002" --conf-file-option "collision_loop: 20" - --conf-file-option "collision_pair: WAIST:LLEG_JOINT2 WAIST:RLEG_JOINT2 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5" + --conf-file-option "collision_pair: LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5" --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)" --conf-file-option "seq_optional_data_dim: 4" ) From 73ec5a2891d28604a9dc2a3ab6d335199c0c9b2b Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 8 Feb 2019 21:02:43 +0900 Subject: [PATCH 18/83] up --- .../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 14a05fc0..bb1342b1 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 @@ -640,7 +640,7 @@ def setStAbcParametersCHIDORI (self): # autost setting stp=self.abc_svc.getStabilizerParam() stp.is_estop_while_walking=True - # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP + stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.k_brot_p=[0, 0] From 683c19739ee3d35e4ef747151efb7ed4e893c274 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 25 Apr 2019 17:00:28 +0900 Subject: [PATCH 19/83] up --- .../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 bb1342b1..d4fb4a4d 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 @@ -194,7 +194,7 @@ def setStAbcIcParametersJAXON(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 # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] @@ -315,8 +315,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_swing_damping_moment_thre=[15]*3 stp.eefm_use_swing_damping=True # stp.eefm_ee_error_cutoff_freq=10000 # not used - stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0]]*4 - stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0]]*4 + stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] stp.eefm_swing_rot_time_const=[[10.0, 10.0, 10.0]]*4 stp.eefm_swing_pos_time_const=[[10.0, 10.0, 10.0]]*4 stp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] From 413783cad2bd4a8edf8ea7d5c275ae5fe64d1e35 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 25 Apr 2019 17:08:47 +0900 Subject: [PATCH 20/83] up --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 d4fb4a4d..68c32f7e 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 @@ -194,7 +194,7 @@ def setStAbcIcParametersJAXON(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 # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] @@ -273,7 +273,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP - stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 + # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 stp.cp_check_margin=[0.05, 0.045, 0, 0.095] stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] From abd4e3b8b7c40918288145147001b33972230e41 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 17 May 2019 19:20:40 +0900 Subject: [PATCH 21/83] up --- .../urata_hrpsys_config.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 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 68c32f7e..baba8c6a 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 @@ -180,9 +180,9 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; if self.ROBOT_NAME == "JAXON": - 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.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; elif self.ROBOT_NAME == "JAXON_RED": - abcp.default_zmp_offsets=[[0.0, 0.01, 0.0], [0.0, -0.01, 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=1.0 self.abc_svc.setAutoBalancerParam(abcp) # kf setting @@ -313,12 +313,12 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.use_zmp_truncation=True stp.eefm_swing_damping_force_thre=[200]*3 stp.eefm_swing_damping_moment_thre=[15]*3 - stp.eefm_use_swing_damping=True + stp.eefm_use_swing_damping=False # stp.eefm_ee_error_cutoff_freq=10000 # not used stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - stp.eefm_swing_rot_time_const=[[10.0, 10.0, 10.0]]*4 - stp.eefm_swing_pos_time_const=[[10.0, 10.0, 10.0]]*4 + stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 + stp.eefm_swing_pos_time_const=[[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 @@ -657,7 +657,7 @@ def setStAbcParametersCHIDORI (self): [16800.0, 16800.0, 7500.0]] stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 - stp.eefm_use_swing_damping=True + stp.eefm_use_swing_damping=False 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_compensation_limit = [math.radians(30), math.radians(30)] @@ -665,8 +665,8 @@ def setStAbcParametersCHIDORI (self): # stp.eefm_ee_error_cutoff_freq=10000 # not used stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0]]*2 stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0]]*2 - stp.eefm_swing_rot_time_const=[[10.0, 10.0, 10.0]]*2 - stp.eefm_swing_pos_time_const=[[10.0, 10.0, 10.0]]*2 + stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*2 + stp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*2 stp.eefm_wrench_alpha_blending=0.7 stp.eefm_pos_time_const_swing=0.06 stp.eefm_pos_transition_time=0.01 From 80f80b3399fd7541272296ad588c5ee1335d0064 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 14 Jul 2019 16:17:33 +0900 Subject: [PATCH 22/83] up --- .../urata_hrpsys_config.py | 145 +++++++++--------- 1 file changed, 76 insertions(+), 69 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 baba8c6a..bf17db7f 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 @@ -269,97 +269,104 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): stp.eefm_k3=[-0.198033,-0.198033] self.st_svc.setParameter(stp) # AutoSt setting - stp=self.abc_svc.getStabilizerParam() - #stp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - # stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP - # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 - stp.cp_check_margin=[0.05, 0.045, 0, 0.095] - stp.k_brot_p=[0, 0] - stp.k_brot_tc=[1000, 1000] - #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] + astp=self.abc_svc.getStabilizerParam() + #astp.st_algorithm=OpenHRP.AutoBalancerService.EEFM + # astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + astp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 + astp.cp_check_margin=[0.05, 0.045, 0, 0.095] + astp.k_brot_p=[0, 0] + astp.k_brot_tc=[1000, 1000] + #astp.eefm_body_attitude_control_gain=[0, 0.5] + astp.eefm_body_attitude_control_gain=[0.5, 0.5] + astp.eefm_body_attitude_control_time_const=[1000, 1000] if self.ROBOT_NAME == "JAXON": - stp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5*1.2*1.65*1.1, 20*1.6*1.1*1.5*1.2*1.65*1.1, 1e5]]*4 - stp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5*1.2*1.1]]*4 - stp.eefm_swing_rot_damping_gain=[20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5] - stp.eefm_swing_pos_damping_gain=[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.4] - stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] - stp.eefm_pos_compensation_limit = [0.06, 0.06, 0.050, 0.050] + astp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5*1.2*1.65*1.1, 20*1.6*1.1*1.5*1.2*1.65*1.1, 1e5]]*4 + astp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5*1.2*1.1]]*4 + astp.eefm_swing_rot_damping_gain=[20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5] + astp.eefm_swing_pos_damping_gain=[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.4] + astp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] + astp.eefm_pos_compensation_limit = [0.06, 0.06, 0.050, 0.050] elif self.ROBOT_NAME == "JAXON_RED": - stp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5, 20*1.6*1.1*1.5, 1e5], + astp.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], + astp.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_rot_damping_gain = [[1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], + # astp.eefm_rot_damping_gain = [[1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], # [1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], # [1e5*20*1.6*1.1*1.5*1.2, 1e5*20*1.6*1.1*1.5*1.2, 1e5], # [1e5*20*1.6*1.1*1.5*1.2, 1e5*20*1.6*1.1*1.5*1.2, 1e5]] - # stp.eefm_pos_damping_gain = [[1e5*3500*1.6*6, 1e5*3500*1.6*6, 1e5*3500*1.6*1.1*1.5], + # astp.eefm_pos_damping_gain = [[1e5*3500*1.6*6, 1e5*3500*1.6*6, 1e5*3500*1.6*1.1*1.5], # [1e5*3500*1.6*6, 1e5*3500*1.6*6, 1e5*3500*1.6*1.1*1.5], # [1e5*3500*1.6*6*0.8, 1e5*3500*1.6*6*0.8, 1e5*3500*1.6*1.1*1.5*0.8], # [1e5*3500*1.6*6*0.8, 1e5*3500*1.6*6*0.8, 1e5*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_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] - stp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] - stp.eefm_zmp_delay_time_const=[0, 0] - stp.detection_time_to_air=5.0 - stp.use_zmp_truncation=True - stp.eefm_swing_damping_force_thre=[200]*3 - stp.eefm_swing_damping_moment_thre=[15]*3 - stp.eefm_use_swing_damping=False - # stp.eefm_ee_error_cutoff_freq=10000 # not used - stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 - stp.eefm_swing_pos_time_const=[[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 - stp.eefm_wrench_alpha_blending=0.7 - stp.eefm_pos_time_const_swing=0.06 - stp.eefm_pos_transition_time=0.01 - stp.eefm_pos_margin_time=0.02 + astp.eefm_swing_pos_damping_gain = astp.eefm_pos_damping_gain[0] + astp.eefm_swing_rot_damping_gain = astp.eefm_rot_damping_gain[0] + astp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] + astp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] + astp.eefm_zmp_delay_time_const=[0, 0] + astp.detection_time_to_air=5.0 + # astp.use_zmp_truncation=True + astp.eefm_swing_damping_force_thre=[200]*3 + astp.eefm_swing_damping_moment_thre=[15]*3 + astp.eefm_use_swing_damping=True + # astp.eefm_ee_error_cutoff_freq=10000 # not used + astp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_rot_spring_gain=[[20.0, 20.0, 20.0], [20.0, 20.0, 20.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_pos_spring_gain=[[20.0, 20.0, 20.0], [20.0, 20.0, 20.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 + astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 + astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] + astp.eefm_rot_time_const = [[1.5/1.1, 1.5/1.1, 1.5/1.1]]*4 + astp.eefm_pos_time_const_support = [[3.0/1.1, 3.0/1.1, 1.5/1.1]]*4 + astp.eefm_wrench_alpha_blending=0.7 + astp.eefm_pos_time_const_swing=0.06 + astp.eefm_pos_transition_time=0.01 + astp.eefm_pos_margin_time=0.02 # foot margin param if foot == "KAWADA": ## KAWADA foot : mechanical param is => inside 0.055, front 0.13, rear 0.1 - 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 + # astp.eefm_leg_inside_margin=0.05 + # astp.eefm_leg_outside_margin=0.05 + # astp.eefm_leg_front_margin=0.12 + # astp.eefm_leg_rear_margin=0.09 + ## with mergin + astp.eefm_leg_inside_margin=0.04 + astp.eefm_leg_outside_margin=0.04 + astp.eefm_leg_front_margin=0.1 + astp.eefm_leg_rear_margin=0.07 elif foot == "JSK": ## JSK foot : mechanical param is -> inside 0.075, front 0.11, rear 0.11 - stp.eefm_leg_inside_margin=0.07 - stp.eefm_leg_outside_margin=0.07 - stp.eefm_leg_front_margin=0.1 - stp.eefm_leg_rear_margin=0.1 + astp.eefm_leg_inside_margin=0.07 + astp.eefm_leg_outside_margin=0.07 + astp.eefm_leg_front_margin=0.1 + astp.eefm_leg_rear_margin=0.1 elif foot == "LEPTRINO": - stp.eefm_leg_inside_margin=0.05 - stp.eefm_leg_outside_margin=0.05 - stp.eefm_leg_front_margin=0.115 - stp.eefm_leg_rear_margin=0.115 - rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_inside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_outside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_outside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_inside_margin])] - lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_outside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] + astp.eefm_leg_inside_margin=0.05 + astp.eefm_leg_outside_margin=0.05 + astp.eefm_leg_front_margin=0.115 + astp.eefm_leg_rear_margin=0.115 + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, -1*astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, -1*astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, astp.eefm_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, -1*astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, -1*astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, astp.eefm_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.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] - self.abc_svc.setStabilizerParam(stp) + astp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + astp.eefm_cogvel_cutoff_freq = 4.0 + astp.eefm_k1=[-1.48412,-1.48412] + astp.eefm_k2=[-0.486727,-0.486727] + astp.eefm_k3=[-0.198033,-0.198033] + self.abc_svc.setStabilizerParam(astp) # Abc setting #gg=self.abc_svc.getGaitGeneratorParam()[1] #gg.stride_parameter=[0.1,0.05,10.0] From 702fc4f575f2cef4c8688af01bd6fd2e3852ceb6 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sat, 24 Aug 2019 17:59:47 +0900 Subject: [PATCH 23/83] up --- .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 8 +++----- 1 file changed, 3 insertions(+), 5 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 bf17db7f..2c212315 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 @@ -317,8 +317,6 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # astp.eefm_ee_error_cutoff_freq=10000 # not used astp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - # astp.eefm_swing_rot_spring_gain=[[20.0, 20.0, 20.0], [20.0, 20.0, 20.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - # astp.eefm_swing_pos_spring_gain=[[20.0, 20.0, 20.0], [20.0, 20.0, 20.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] @@ -389,7 +387,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; - gg.optional_go_pos_finalize_footstep_num=1 + gg.optional_go_pos_finalize_footstep_num=0 gg.overwritable_footstep_index_offset=0 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting @@ -401,7 +399,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): self.ic_svc.setImpedanceControllerParam(l, icp) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] - esp.default_recover_time=10.0 # [s] + esp.default_recover_time=3.0 # [s] esp.default_retrieve_time=1.0 # [s] self.es_svc.setEmergencyStopperParam(esp) @@ -582,7 +580,7 @@ def setStAbcParametersCHIDORI (self): stp.k_brot_p=[0, 0] stp.k_brot_tc=[1000, 1000] stp.eefm_body_attitude_control_gain=[0.5, 0.5] - stp.eefm_body_attitude_control_time_const=[1000, 1000] + stp.eefm_body_attitude_control_time_const=[1e5, 1e5] stp.eefm_rot_damping_gain=[[20*1.6*1.1*1.5, 20*1.6*1.1*1.5*1.5, 1e5]]*2 stp.eefm_pos_damping_gain=[[3500*1.6*3, 3500*1.6*3, 3500*1.6*1.1*1.5*0.5]]*2 stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 From 02c2e00de28acd3770e7200feab97b22bfc35762 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 29 Nov 2019 18:20:08 +0900 Subject: [PATCH 24/83] up --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 2c212315..618d763a 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 @@ -315,8 +315,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - astp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0], [10.0, 10.0, 10.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] From 3488b12f2a757ddba619c0238ed04945f9888c45 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 9 Jan 2020 10:44:28 +0900 Subject: [PATCH 25/83] add fullbody for jaxon --- .../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 618d763a..30996cb9 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 @@ -184,6 +184,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): elif self.ROBOT_NAME == "JAXON_RED": 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=1.0 + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] From f85fe3ab0632857440ef3280844f572de3259d85 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 9 Jan 2020 12:00:59 +0900 Subject: [PATCH 26/83] overwritable_footstep_index_offset=1 --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 5 +++-- 1 file changed, 3 insertions(+), 2 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 30996cb9..9d795649 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 @@ -389,7 +389,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; gg.optional_go_pos_finalize_footstep_num=0 - gg.overwritable_footstep_index_offset=0 + gg.overwritable_footstep_index_offset=1 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting limbs = ['rarm', 'larm'] @@ -736,7 +736,8 @@ def setStAbcParametersCHIDORI (self): gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; - gg.overwritable_footstep_index_offset=0 + gg.optional_go_pos_finalize_footstep_num=0 + gg.overwritable_footstep_index_offset=1 self.abc_svc.setGaitGeneratorParam(gg) def setStAbcParametersTQLEG0 (self): From 0e59650fe93037a74d5ba905a95a37d1b77d4c4a Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 27 Feb 2020 21:34:05 +0900 Subject: [PATCH 27/83] up --- .../urata_hrpsys_config.py | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 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 9d795649..8f4045f3 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 @@ -337,7 +337,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): ## with mergin astp.eefm_leg_inside_margin=0.04 astp.eefm_leg_outside_margin=0.04 - astp.eefm_leg_front_margin=0.1 + astp.eefm_leg_front_margin=0.07 astp.eefm_leg_rear_margin=0.07 elif foot == "JSK": ## JSK foot : mechanical param is -> inside 0.075, front 0.11, rear 0.11 @@ -568,8 +568,9 @@ def setStAbcParametersCHIDORI (self): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; - abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0]]; # 20170704 + abcp.default_zmp_offsets=[[0.05, 0.0, 0.0], [0.05, 0.0, 0.0]]; # 20170704 abcp.move_base_gain=1.2 + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] @@ -663,14 +664,14 @@ def setStAbcParametersCHIDORI (self): [16800.0, 16800.0, 7500.0]] stp.eefm_rot_time_const=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 - stp.eefm_use_swing_damping=False + stp.eefm_use_swing_damping=True 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_compensation_limit = [math.radians(30), math.radians(30)] stp.eefm_pos_compensation_limit = [0.05, 0.05] # stp.eefm_ee_error_cutoff_freq=10000 # not used - stp.eefm_swing_rot_spring_gain=[[10.0, 10.0, 10.0]]*2 - stp.eefm_swing_pos_spring_gain=[[10.0, 10.0, 10.0]]*2 + stp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0]]*2 + stp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0]]*2 stp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*2 stp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*2 stp.eefm_wrench_alpha_blending=0.7 @@ -724,7 +725,7 @@ def setStAbcParametersCHIDORI (self): gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=1.2 #gg.default_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 + gg.default_double_support_ratio=0.2 #gg.stride_parameter=[0.1,0.05,10.0] #gg.default_step_time=1.0 #gg.swing_trajectory_delay_time_offset=0.35 @@ -738,6 +739,18 @@ def setStAbcParametersCHIDORI (self): gg.heel_zmp_offset_x = 1e-3*-116.342; gg.optional_go_pos_finalize_footstep_num=0 gg.overwritable_footstep_index_offset=1 + gg.leg_margin = [0.15, 0.09, 0.065, 0.065] + gg.safe_leg_margin = [0.1, 0.0, 0.05, 0.05] + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.15, 0.13] + gg.overwritable_stride_limitation = [0.25, 0.3, 0, 0.3, 0.13] + gg.margin_time_ratio = 0.2 + gg.min_time_margin = 0.3 + gg.min_time = 1.0 + gg.zmp_delay_time_const = 0.06 + gg.fg_zmp_cutoff_freq = 10 + gg.fg_cp_cutoff_freq = 10 + gg.dcm_offset = 0.01 self.abc_svc.setGaitGeneratorParam(gg) def setStAbcParametersTQLEG0 (self): From 5c67dd67ddb1bf81b77b607539dcb817a5aafbaf Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 28 Feb 2020 18:18:29 +0900 Subject: [PATCH 28/83] up --- .../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 8f4045f3..d6ad3c3c 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 @@ -710,6 +710,7 @@ def setStAbcParametersCHIDORI (self): OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.cp_check_margin=[0.03, 0.03, 0, 0.07] # stp.eefm_zmp_delay_time_const=[0.055, 0.055] stp.eefm_cogvel_cutoff_freq = 4.0 # calculated by calculate-eefm-st-state-feedback-default-gain-from-robot *chidori* From 4f12043a1f5991f20abe78fd4520cac5a9c2bb15 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 28 Feb 2020 18:23:56 +0900 Subject: [PATCH 29/83] up --- .../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 d6ad3c3c..505163ce 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 @@ -568,7 +568,7 @@ def setStAbcParametersCHIDORI (self): # abc setting abcp=self.abc_svc.getAutoBalancerParam()[1] #abcp.default_zmp_offsets=[[0.015, 0.0, 0.0], [0.015, 0.0, 0.0]]; - abcp.default_zmp_offsets=[[0.05, 0.0, 0.0], [0.05, 0.0, 0.0]]; # 20170704 + abcp.default_zmp_offsets=[[0.05, 0.02, 0.0], [0.05, -0.02, 0.0]]; # 20170704 abcp.move_base_gain=1.2 abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) From 21c43d23d5118985d4392b76bddeb9a8238b379f Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 14 May 2020 11:32:05 +0900 Subject: [PATCH 30/83] [hrpsys_gazebo_tutorials] enable HRP3HAND simulation --- hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml | 26 +----- .../config/HRP2JSKNTS.yaml | 26 +----- hrpsys_gazebo_tutorials/config/HRP3HAND.yaml | 42 ++++++++++ .../gazebo_hrp2jsknt_no_controllers.launch | 4 +- .../gazebo_hrp2jsknts_no_controllers.launch | 4 +- .../launch/hrp2jsknt_hrpsys_bringup.launch | 23 ++++++ .../launch/hrp2jsknts_hrpsys_bringup.launch | 23 ++++++ .../HRP2JSKNT/HRP2JSKNT.urdf.xacro | 6 ++ .../HRP2JSKNTS/HRP2JSKNTS.urdf.xacro | 6 ++ .../hrp2_hrpsys_config.py | 81 ++++++++++++++++++- 10 files changed, 183 insertions(+), 58 deletions(-) create mode 100644 hrpsys_gazebo_tutorials/config/HRP3HAND.yaml diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml index b0ed9245..6708f073 100644 --- a/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml @@ -11,7 +11,7 @@ hrpsys_gazebo_configuration: ### name of robot (using for namespace) robotname: HRP2JSKNT ### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id - joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45] + joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33] ### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) joints: - RLEG_JOINT0 @@ -48,18 +48,6 @@ hrpsys_gazebo_configuration: - LARM_JOINT5 - LARM_JOINT6 - LARM_JOINT7 - - L_THUMBCM_Y - - L_THUMBCM_P - - L_INDEXMP_R - - L_INDEXMP_P - - L_INDEXPIP_R - - L_MIDDLEPIP_R - - R_THUMBCM_Y - - R_THUMBCM_P - - R_INDEXMP_R - - R_INDEXMP_P - - R_INDEXPIP_R - - R_MIDDLEPIP_R ## jointid: gains: CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} @@ -94,18 +82,6 @@ hrpsys_gazebo_configuration: RLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0} - L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} force_torque_sensors: - rfsensor diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml index 67ad5115..bb7899b5 100644 --- a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml +++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml @@ -11,7 +11,7 @@ hrpsys_gazebo_configuration: ### name of robot (using for namespace) robotname: HRP2JSKNTS ### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id - joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45] + joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33] ### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) joints: - RLEG_JOINT0 @@ -48,18 +48,6 @@ hrpsys_gazebo_configuration: - LARM_JOINT5 - LARM_JOINT6 - LARM_JOINT7 - - L_THUMBCM_Y - - L_THUMBCM_P - - L_INDEXMP_R - - L_INDEXMP_P - - L_INDEXPIP_R - - L_MIDDLEPIP_R - - R_THUMBCM_Y - - R_THUMBCM_P - - R_INDEXMP_R - - R_INDEXMP_P - - R_INDEXPIP_R - - R_MIDDLEPIP_R ## jointid: gains: CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} @@ -94,18 +82,6 @@ hrpsys_gazebo_configuration: RLEG_JOINT4: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT5: {p: 1000.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0} - L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} - R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} force_torque_sensors: - rfsensor diff --git a/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml b/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml new file mode 100644 index 00000000..5027d3ca --- /dev/null +++ b/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml @@ -0,0 +1,42 @@ +hrp3hand_hrpsys_gazebo_configuration: +### velocity feedback for joint control, use parameter gains/joint_name/p_v + use_velocity_feedback: false + use_joint_effort: true + iob_rate: 100 +### loose synchronization default true + use_loose_synchronized: false +### synchronized hrpsys and gazebo +# use_synchronized_command: false +# iob_substeps: 5 +### name of robot (using for namespace) + robotname: HRP3HAND +### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id + joint_id_list: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] +### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints) + joints: + - R_THUMBCM_Y + - R_THUMBCM_P + - R_INDEXMP_R + - R_INDEXMP_P + - R_INDEXPIP_R + - R_MIDDLEPIP_R + - L_THUMBCM_Y + - L_THUMBCM_P + - L_INDEXMP_R + - L_INDEXMP_P + - L_INDEXPIP_R + - L_MIDDLEPIP_R +## jointid: + gains: + R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} + L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0} diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers.launch index 8e86ff08..f526991c 100644 --- a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers.launch +++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknt_no_controllers.launch @@ -5,9 +5,7 @@ - + file="$(find hrpsys_gazebo_tutorials)/config/HRP3HAND.yaml" ns="HRP3HAND" /> diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch index 119e6074..f5b02a55 100644 --- a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch +++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch @@ -5,9 +5,7 @@ - + file="$(find hrpsys_gazebo_tutorials)/config/HRP3HAND.yaml" ns="HRP3HAND" /> diff --git a/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch index b910f029..c88b961d 100644 --- a/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/hrp2jsknt_hrpsys_bringup.launch @@ -15,6 +15,11 @@ + + + + + @@ -23,5 +28,23 @@ + + + + + + + + + + + + + + + + + diff --git a/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch b/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch index 48290830..b630fbba 100644 --- a/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch +++ b/hrpsys_gazebo_tutorials/launch/hrp2jsknts_hrpsys_bringup.launch @@ -16,6 +16,11 @@ + + + + + @@ -24,5 +29,23 @@ + + + + + + + + + + + + + + + + + diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro index 74a31d9b..2e14571c 100644 --- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNT/HRP2JSKNT.urdf.xacro @@ -7,6 +7,12 @@ hrpsys_gazebo_configuration + + + HRP3HAND + hrp3hand_hrpsys_gazebo_configuration + + diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro index 91fe1018..33495f67 100644 --- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro +++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro @@ -7,6 +7,12 @@ hrpsys_gazebo_configuration + + + HRP3HAND + hrp3hand_hrpsys_gazebo_configuration + + diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3bf0d267..2eca01ad 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -7,11 +7,64 @@ from hrpsys.hrpsys_config import * import OpenHRP +pkg = 'jsk_hrp2_ros_bridge' +import imp +try: + imp.find_module(pkg) +except: + print "No such package ", pkg + +try: + from jsk_hrp2_ros_bridge.HRP3HandControllerService_idl import * +except: + print """No HRP3hand module is availabe on this machine, +it may cause some error""" + class JSKHRP2HrpsysConfigurator(HrpsysConfigurator): ROBOT_NAME = None - def getRTCList (self): - return self.getRTCListUnstable() + hc = None + hc_svc = None + + def connectComps(self): + HrpsysConfigurator.connectComps(self) + if self.rh.port("servoState") != None: + if self.hc: + connectPorts(self.rh.port("servoState"), self.hc.port("servoStateIn")) + + def getRTCList(self): + rtclist = [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + #['tf', "TorqueFilter"], + ['kf', "KalmanFilter"], + #['vs', "VirtualForceSensor"], + ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], + ['es', "EmergencyStopper"], + ['rfu', "ReferenceForceUpdater"], + ['ic', "ImpedanceController"], + ['abc', "AutoBalancer"], + ['st', "Stabilizer"], + ['co', "CollisionDetector"], + #['tc', "TorqueController"], + ['te', "ThermoEstimator"], + ['hes', "EmergencyStopper"], + ['el', "SoftErrorLimiter"], + ['tl', "ThermoLimiter"], + ['bp', "Beeper"], + ['log', "DataLogger"], + ] + if self.ROBOT_NAME.find("HRP2JSKNT") != -1: + rtclist.append(['hc', "HRP3HandController"]) + if self.ROBOT_NAME.find("HRP2JSKNT") == -1: # if HRP2W, HRP2G and HRP2JSK + tmpidx=rtclist.index(['ic', "ImpedanceController"]) + rtclist[0:tmpidx+1]+rtclist[tmpidx+1:] + rtclist=rtclist[0:tmpidx+1]+[['gc', "GraspController"]]+rtclist[tmpidx+1:] + print >>sys.stderr, "RTC ", rtclist, "[",self.ROBOT_NAME.find("HRP2JSKNT"),"][",self.ROBOT_NAME,"]" + return rtclist + def init (self, robotname="Robot", url=""): HrpsysConfigurator.init(self, robotname, url) print "initialize rtc parameters" @@ -382,6 +435,30 @@ def loadForceMomentOffsetFile (self): else: print "No force moment offset file" + def hrp3HandResetPose(self): + self.hc_svc.setJointAngles([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 2) + self.hc_svc.waitInterpolation() + + def hrp3HandGraspPose(self): + self.hc_svc.setJointAngles([77.9709, -11.4732, 8.28742, 0.0, 106.185, 86.0974, 77.9709, -11.4732, 8.28742, 0.0, 106.185, 86.0974], 2) + self.hc_svc.waitInterpolation() + + def hrp3HandHookPose(self): + self.hc_svc.setJointAngles([90.0, 90.0, 0.0, 10.0, -20.0, -20.0, 90.0, 90.0, 0.0, 10.0, -20.0, -20.0], 2) + self.hc_svc.waitInterpolation() + + def hrp3HandCalib(self): + self.hc_svc.handJointCalib() + + def hrp3HandServoOn(self): + self.hc_svc.handServoOn() + + def hrp3HandServoOff(self): + self.hc_svc.handServoOff() + + def hrp3HandReconnect(self): + self.hc_svc.handReconnect() + def __init__(self, robotname=""): self.ROBOT_NAME = robotname HrpsysConfigurator.__init__(self) From 0f650bfba2165127d1d0f89312e02d44b2851319 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Tue, 2 Jun 2020 17:11:58 +0900 Subject: [PATCH 31/83] remove st setting for JAXON --- .../urata_hrpsys_config.py | 81 +------------------ 1 file changed, 1 insertion(+), 80 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 505163ce..e5948b8f 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 @@ -19,7 +19,7 @@ def getRTCList (self): def init (self, robotname="Robot", url=""): HrpsysConfigurator.init(self, robotname, url) - if self.st and self.abc and self.kf and self.ic and self.es and self.el: + if self.abc and self.kf and self.ic and self.es and self.el: self.setStAbcParameters() if self.rmfo: self.loadForceMomentOffsetFile() @@ -190,85 +190,6 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): kfp=self.kf_svc.getKalmanFilterParam()[1] kfp.R_angle=1000 self.kf_svc.setKalmanFilterParam(kfp) - # st setting - stp=self.st_svc.getParameter() - #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.cp_check_margin=[0.05, 0.045, 0, 0.095] - stp.k_brot_p=[0, 0] - stp.k_brot_tc=[1000, 1000] - #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] - if self.ROBOT_NAME == "JAXON": - stp.eefm_rot_damping_gain = [[20*1.6*1.1*1.5*1.2*1.65*1.1, 20*1.6*1.1*1.5*1.2*1.65*1.1, 1e5]]*4 - stp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.1*1.5*1.2*1.1]]*4 - stp.eefm_swing_rot_damping_gain=[20*1.6*1.1*1.5*1.2, 20*1.6*1.1*1.5*1.2, 1e5] - stp.eefm_swing_pos_damping_gain=[3500*1.6*6, 3500*1.6*6, 3500*1.6*1.4] - stp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] - stp.eefm_pos_compensation_limit = [0.06, 0.06, 0.050, 0.050] - elif self.ROBOT_NAME == "JAXON_RED": - 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_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_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 - stp.eefm_wrench_alpha_blending=0.7 - stp.eefm_pos_time_const_swing=0.06 - stp.eefm_pos_transition_time=0.01 - stp.eefm_pos_margin_time=0.02 - # foot margin param - if foot == "KAWADA": - ## KAWADA foot : mechanical param is => inside 0.055, front 0.13, rear 0.1 - 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 - elif foot == "JSK": - ## JSK foot : mechanical param is -> inside 0.075, front 0.11, rear 0.11 - stp.eefm_leg_inside_margin=0.07 - stp.eefm_leg_outside_margin=0.07 - stp.eefm_leg_front_margin=0.1 - stp.eefm_leg_rear_margin=0.1 - elif foot == "LEPTRINO": - stp.eefm_leg_inside_margin=0.05 - stp.eefm_leg_outside_margin=0.05 - stp.eefm_leg_front_margin=0.115 - stp.eefm_leg_rear_margin=0.115 - 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]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] - rarm_vertices = rleg_vertices - 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] - self.st_svc.setParameter(stp) # AutoSt setting astp=self.abc_svc.getStabilizerParam() #astp.st_algorithm=OpenHRP.AutoBalancerService.EEFM From eed6c7047695ed7316ce4f958d205ee35b209d99 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 5 Jun 2020 14:43:47 +0900 Subject: [PATCH 32/83] set optional_go_pos_finalize_footstep_num=1 --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 e5948b8f..a68852ca 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 @@ -309,7 +309,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; - gg.optional_go_pos_finalize_footstep_num=0 + gg.optional_go_pos_finalize_footstep_num=1 gg.overwritable_footstep_index_offset=1 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting @@ -659,7 +659,7 @@ def setStAbcParametersCHIDORI (self): gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; - gg.optional_go_pos_finalize_footstep_num=0 + gg.optional_go_pos_finalize_footstep_num=1 gg.overwritable_footstep_index_offset=1 gg.leg_margin = [0.15, 0.09, 0.065, 0.065] gg.safe_leg_margin = [0.1, 0.0, 0.05, 0.05] From fbdc2135ce2f7debd3d54824f9fa3298f931f892 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Fri, 30 Oct 2020 17:51:19 +0900 Subject: [PATCH 33/83] [hrpsys_ros_bridge_tutorials] fix hand_force_calib_offset_HRP2JSKNT* (add HRP3Hand) --- .../models/hand_force_calib_offset_HRP2JSKNT | 4 ++-- .../models/hand_force_calib_offset_HRP2JSKNTS | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNT b/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNT index 7c39b0df..696d7897 100644 --- a/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNT +++ b/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNT @@ -1,4 +1,4 @@ lfsensor 0 0 0 0 0 0 0 0 0 0 -lhsensor -4.70102e-05 -1.1095e-05 -5.77513e-05 -4.75929e-07 3.98957e-07 2.82246e-07 0.000670833 0.00738842 -0.00822197 1.33256 +lhsensor 0 0 0 0 0 0 0.0053811 0.00222186 -0.0248345 1.66181 rfsensor 0 0 0 0 0 0 0 0 0 0 -rhsensor -4.73635e-05 1.00636e-05 -5.62558e-05 5.19083e-07 4.63629e-07 -3.43949e-07 0.000670838 -0.00738841 -0.00822196 1.33256 +rhsensor 0 0 0 0 0 0 0.0053811 -0.00222186 -0.0248345 1.66181 diff --git a/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNTS b/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNTS index 9b09042c..696d7897 100644 --- a/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNTS +++ b/hrpsys_ros_bridge_tutorials/models/hand_force_calib_offset_HRP2JSKNTS @@ -1,4 +1,4 @@ lfsensor 0 0 0 0 0 0 0 0 0 0 -lhsensor 7.84813e-07 -4.22156e-07 -3.46438e-07 -2.87758e-09 -1.63124e-08 6.2948e-09 0.000670851 0.00738842 -0.00822196 1.33256 +lhsensor 0 0 0 0 0 0 0.0053811 0.00222186 -0.0248345 1.66181 rfsensor 0 0 0 0 0 0 0 0 0 0 -rhsensor -2.77969e-08 -3.79513e-06 -9.67615e-07 -3.06909e-08 -4.84024e-09 1.11543e-08 0.00067085 -0.00738842 -0.00822196 1.33256 +rhsensor 0 0 0 0 0 0 0.0053811 -0.00222186 -0.0248345 1.66181 From 55776595543430439d2c3ef654901537ddf6e8b0 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Mon, 31 Oct 2016 09:41:04 +0900 Subject: [PATCH 34/83] [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 e6b58a4c..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 -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 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 51dceecb..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 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 -33000 240 -83000 240 -33000 240 -33000 240 -47000 240 -33000 240 +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 4e9172eea44d07250910d18ca2f60ce11e7e8241 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 9 Nov 2020 01:29:45 +0900 Subject: [PATCH 35/83] [hrpsys_ros_bridge_tutorials] add torque control setting for JAXON_RED --- .../urata_hrpsys_config.py | 11 +++++++++++ 1 file changed, 11 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 a68852ca..6094139c 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 @@ -286,7 +286,18 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_k1=[-1.48412,-1.48412] astp.eefm_k2=[-0.486727,-0.486727] astp.eefm_k3=[-0.198033,-0.198033] + astp.swing2landing_transition_time = 0.05 + astp.landing_phase_time = 0.1 + astp.landing2support_transition_time = 0.5 + leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1]} + arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], + "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100]} + astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) + # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) + # rh setting + if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: + 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 b1ab989b20171db211f82462199232456936e914 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 11 Nov 2020 19:38:24 +0900 Subject: [PATCH 36/83] add new line --- .travis | 2 +- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis b/.travis index a2a59373..74e96892 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit a2a59373e3a98d064dbd11452179328bfc110e60 +Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 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 6094139c..f679ce4c 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 @@ -289,7 +289,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.swing2landing_transition_time = 0.05 astp.landing_phase_time = 0.1 astp.landing2support_transition_time = 0.5 - leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1]} + leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], + "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1]} arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100]} astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) From b13257adec7e5abf51d21e3c90616b996ce9b524 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 11 Nov 2020 21:25:07 +0900 Subject: [PATCH 37/83] add swing phase pdgains --- .../urata_hrpsys_config.py | 12 ++++++++---- 1 file changed, 8 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 f679ce4c..f94fd2c8 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 @@ -287,12 +287,16 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_k2=[-0.486727,-0.486727] astp.eefm_k3=[-0.198033,-0.198033] astp.swing2landing_transition_time = 0.05 - astp.landing_phase_time = 0.1 - astp.landing2support_transition_time = 0.5 + astp.landing_phase_time = 0.25 + astp.landing2support_transition_time = 0.05 + astp.surpport_phase_min_time = 0.4 + astp.support2swing_transition_time = 0.1 leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], - "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1]} + "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1], + "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], - "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100]} + "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], + "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) From d4f23c6e175b245e805d02b2bea52858ebdc42e7 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 11 Nov 2020 21:32:14 +0900 Subject: [PATCH 38/83] up --- .../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 f94fd2c8..4275a3d9 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 @@ -235,7 +235,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # astp.use_zmp_truncation=True astp.eefm_swing_damping_force_thre=[200]*3 astp.eefm_swing_damping_moment_thre=[15]*3 - astp.eefm_use_swing_damping=True + astp.eefm_use_swing_damping=False # astp.eefm_ee_error_cutoff_freq=10000 # not used astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] @@ -287,7 +287,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_k2=[-0.486727,-0.486727] astp.eefm_k3=[-0.198033,-0.198033] astp.swing2landing_transition_time = 0.05 - astp.landing_phase_time = 0.25 + astp.landing_phase_time = 0.3 astp.landing2support_transition_time = 0.05 astp.surpport_phase_min_time = 0.4 astp.support2swing_transition_time = 0.1 @@ -298,7 +298,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) - # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE + astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) # rh setting if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: From 968528d185ce3eedf215cbba2f9ac5b5cdd7d599 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Thu, 12 Nov 2020 14:18:47 +0900 Subject: [PATCH 39/83] up --- .travis | 2 +- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis b/.travis index 74e96892..a2a59373 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 +Subproject commit a2a59373e3a98d064dbd11452179328bfc110e60 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 4275a3d9..97863a30 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 @@ -298,7 +298,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) - astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE + # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) # rh setting if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: From 305b9653f6fea69c7c31b330044ec35e10ef0be6 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Thu, 12 Nov 2020 17:29:30 +0900 Subject: [PATCH 40/83] up --- .../urata_hrpsys_config.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 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 97863a30..5918f256 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 @@ -237,8 +237,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=False # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0]]*4 + astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0]]*4 astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] @@ -267,8 +267,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_leg_front_margin=0.1 astp.eefm_leg_rear_margin=0.1 elif foot == "LEPTRINO": - astp.eefm_leg_inside_margin=0.05 - astp.eefm_leg_outside_margin=0.05 + astp.eefm_leg_inside_margin=0.065 + astp.eefm_leg_outside_margin=0.065 astp.eefm_leg_front_margin=0.115 astp.eefm_leg_rear_margin=0.115 rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_inside_margin]), @@ -288,12 +288,13 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_k3=[-0.198033,-0.198033] astp.swing2landing_transition_time = 0.05 astp.landing_phase_time = 0.3 - astp.landing2support_transition_time = 0.05 - astp.surpport_phase_min_time = 0.4 + astp.landing2support_transition_time = 0.1 + astp.surpport_phase_min_time = 0.3 astp.support2swing_transition_time = 0.1 leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1], - "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} + "swing_pgain":[5,30,10,5,10,10], "swing_dgain":[70,70,50,10,10,10]} + # "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} From 7cc0d5f8da293c23d1425fb681ec368aaf54e1c3 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Sun, 29 Nov 2020 16:12:54 +0900 Subject: [PATCH 41/83] use eefmqp --- .../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 5918f256..d3a90268 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 @@ -193,8 +193,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # AutoSt setting astp=self.abc_svc.getStabilizerParam() #astp.st_algorithm=OpenHRP.AutoBalancerService.EEFM - # astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP - astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP astp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 astp.cp_check_margin=[0.05, 0.045, 0, 0.095] astp.k_brot_p=[0, 0] @@ -232,7 +232,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] astp.eefm_zmp_delay_time_const=[0, 0] astp.detection_time_to_air=5.0 - # astp.use_zmp_truncation=True + astp.use_zmp_truncation=True astp.eefm_swing_damping_force_thre=[200]*3 astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=False From b3faf936e463a51d828a9e3428413024daf5a73e Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 30 Nov 2020 16:34:46 +0900 Subject: [PATCH 42/83] rm st rtc for hrp2 --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 2eca01ad..76e26851 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -46,7 +46,7 @@ def getRTCList(self): ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], - ['st', "Stabilizer"], + # ['st', "Stabilizer"], ['co', "CollisionDetector"], #['tc', "TorqueController"], ['te', "ThermoEstimator"], From 4c1cdc3115e16341e54de7a865d2b395fc2ca110 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 30 Nov 2020 16:54:16 +0900 Subject: [PATCH 43/83] set hrp2 astp param --- .../hrp2_hrpsys_config.py | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 76e26851..e161e793 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -129,12 +129,13 @@ def setStAbcParametershrp2017c(self): #abcp.default_zmp_offsets = [[0.015, -0.01, 0], [0.015, 0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.01, 0.01, 0], [0.01, -0.01, 0], [0, 0, 0], [0, 0, 0]] + # abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters - stp=self.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP + stp=self.abc_svc.getStabilizerParam() + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP # eefm st params - stp.eefm_body_attitude_control_gain=[1.5, 1.5] + stp.eefm_body_attitude_control_again=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] # EEFM parameters for 4 limbs #stp.eefm_rot_damping_gain = [[20*1.6, 20*1.6, 1e5]]*4 @@ -150,7 +151,7 @@ def setStAbcParametershrp2017c(self): stp.eefm_pos_time_const_swing=0.08 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_zmp_delay_time_const=[0.0, 0.0] stp.eefm_cogvel_cutoff_freq=6.0 # mechanical foot edge #stp.eefm_leg_inside_margin=0.065 @@ -165,17 +166,17 @@ def setStAbcParametershrp2017c(self): stp.eefm_leg_outside_margin=tmp_leg_outside_margin stp.eefm_leg_front_margin=tmp_leg_front_margin stp.eefm_leg_rear_margin=tmp_leg_rear_margin - rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices 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_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] @@ -189,12 +190,14 @@ def setStAbcParametershrp2017c(self): stp.eefm_k2=[-0.36367379999999999, -0.36367379999999999] stp.eefm_k3=[-0.16200000000000001, -0.16200000000000001] # for estop - stp.emergency_check_mode=OpenHRP.StabilizerService.CP; + # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - self.st_svc.setParameter(stp) + stp.use_zmp_truncation = True + stp.detection_time_to_air = 1.0 + self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=1.1 @@ -209,12 +212,15 @@ def setStAbcParametershrp2017c(self): gg.swing_trajectory_final_distance_weight=1.5 gg.swing_trajectory_time_offset_xy2z=0.1 # [s] # - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + # gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*142.869; gg.heel_pos_offset_x = 1e-3*-105.784; gg.toe_zmp_offset_x = 1e-3*79.411; gg.heel_zmp_offset_x = 1e-3*-105.784; gg.use_toe_joint = True + gg.optional_go_pos_finalize_footstep_num = 1 + gg.overwritable_footstep_index_offset = 1 self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] From 704616bbf85757e4cb23af2d3cdcc9705f7559cf Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Mon, 30 Nov 2020 17:19:45 +0900 Subject: [PATCH 44/83] fix param for jaxonred jikki --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 2 +- .../models/jaxon_red.yaml | 4 ++-- .../urata_hrpsys_config.py | 23 +++++++++++-------- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 92594c02..7ce627e7 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -359,7 +359,7 @@ compile_rbrain_model_for_closed_robots(JAXON_RED JAXON_RED JAXON_RED ## 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_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,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_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,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.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,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_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT7,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_red.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml index b84fb00d..b79c6a8b 100644 --- a/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml +++ b/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml @@ -71,10 +71,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.110] lleg-end-coords: parent: LLEG_LINK5 - translate : [0, 0, -0.100] + translate : [0, 0, -0.110] ## 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 d3a90268..89b7c8fd 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 @@ -61,9 +61,9 @@ def defJointGroups (self): def setStAbcParameters (self): if self.ROBOT_NAME == "STARO": self.setStAbcParametersSTARO() - elif self.ROBOT_NAME == "JAXON": - self.setStAbcIcParametersJAXON(foot="LEPTRINO") elif self.ROBOT_NAME == "JAXON_RED": + self.setStAbcIcParametersJAXON(foot="LEPTRINO") + elif self.ROBOT_NAME == "JAXON": self.setStAbcIcParametersJAXON(foot="KAWADA") elif self.ROBOT_NAME == "JAXON_BLUE": self.setStAbcIcParametersJAXON_BLUE() @@ -210,12 +210,12 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] astp.eefm_pos_compensation_limit = [0.06, 0.06, 0.050, 0.050] elif self.ROBOT_NAME == "JAXON_RED": - astp.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], + astp.eefm_rot_damping_gain = [[120, 120, 1e5], + [120, 120, 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]] - astp.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], + astp.eefm_pos_damping_gain = [[3500*1.6*6, 3500*1.6*6, 10000], + [3500*1.6*6, 3500*1.6*6, 10000], [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]] # astp.eefm_rot_damping_gain = [[1e5*20*1.6*1.1*1.5, 1e5*20*1.6*1.1*1.5, 1e5], @@ -291,10 +291,13 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.landing2support_transition_time = 0.1 astp.surpport_phase_min_time = 0.3 astp.support2swing_transition_time = 0.1 - leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], - "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1], - "swing_pgain":[5,30,10,5,10,10], "swing_dgain":[70,70,50,10,10,10]} - # "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} + # leg_gains = {"support_pgain":[5,30,10,5,0.15,0.12], "support_dgain":[70,70,50,10,0.1,0.1], + # "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1], + # "swing_pgain":[5,30,10,5,10,10], "swing_dgain":[70,70,50,10,10,10]} + # # "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} + leg_gains = {"support_pgain":[5,5,5,5,0.1,0.1], "support_dgain":[70,20,20,20,0.1,0.1], + "landing_pgain":[5,1,1,1,0.1,0.1], "landing_dgain":[70,10,10,10,0.1,0.1], + "swing_pgain":[5,30,10,10,10,10], "swing_dgain":[70,70,50,50,10,10]} arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} From e42b0a67b6896dc9fe9568e7bb9458274adcc8e6 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Mon, 30 Nov 2020 20:02:17 +0900 Subject: [PATCH 45/83] up --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 89b7c8fd..32cb547b 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 @@ -61,9 +61,9 @@ def defJointGroups (self): def setStAbcParameters (self): if self.ROBOT_NAME == "STARO": self.setStAbcParametersSTARO() - elif self.ROBOT_NAME == "JAXON_RED": - self.setStAbcIcParametersJAXON(foot="LEPTRINO") elif self.ROBOT_NAME == "JAXON": + self.setStAbcIcParametersJAXON(foot="LEPTRINO") + elif self.ROBOT_NAME == "JAXON_RED": self.setStAbcIcParametersJAXON(foot="KAWADA") elif self.ROBOT_NAME == "JAXON_BLUE": self.setStAbcIcParametersJAXON_BLUE() From bceaf93152e2c2a235685d54ab316c5a45fcb2af Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 30 Nov 2020 21:49:18 +0900 Subject: [PATCH 46/83] add torque gain --- .../models/HRP2JSK.PDgain.dat | 64 ++++++++--------- .../models/HRP2JSKNT.PDgain.dat | 68 +++++++++---------- .../models/HRP2JSKNTS.PDgain.dat | 68 +++++++++---------- 3 files changed, 100 insertions(+), 100 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat index fe9ae702..70def545 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat @@ -1,32 +1,32 @@ -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -8000 500 -13080 500 -1320 100 -1320 100 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -1400 100 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -1400 100 \ No newline at end of file +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +8000 500 1 0 +13080 500 1 0 +1320 100 1 0 +1320 100 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +1400 100 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +1400 100 1 0 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat index 6fbf55ef..410acb1b 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat @@ -1,34 +1,34 @@ -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -300 30 -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -300 30 -8000 500 -13080 500 -1320 100 -1320 100 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -0 0 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -0 0 \ No newline at end of file +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +300 30 1 0 +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +300 30 1 0 +8000 500 1 0 +13080 500 1 0 +1320 100 1 0 +1320 100 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +0 0 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +0 0 1 0 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat index 6fbf55ef..2e411527 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat @@ -1,34 +1,34 @@ -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -300 30 -1800 500 -21500 500 -8250 500 -18450 500 -4840 500 -3750 500 -300 30 -8000 500 -13080 500 -1320 100 -1320 100 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -0 0 -5400 100 -5550 100 -430 100 -8700 100 -1400 100 -460 100 -1870 100 -0 0 \ No newline at end of file +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +300 30 1 0 +1800 500 1 0 +21500 500 1 0 +8250 500 1 0 +18450 500 1 0 +4840 500 1 0 +3750 500 1 0 +300 30 1 0 +8000 500 1 0 +13080 500 1 0 +1320 100 1 0 +1320 100 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +0 0 1 0 +5400 100 1 0 +5550 100 1 0 +430 100 1 0 +8700 100 1 0 +1400 100 1 0 +460 100 1 0 +1870 100 1 0 +0 0 1 0 From 48b0312073110c4391177d84e86a70dbdf4c9946 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 30 Nov 2020 22:01:56 +0900 Subject: [PATCH 47/83] add hrp2 param --- .../hrp2_hrpsys_config.py | 68 +++++++++++-------- 1 file changed, 39 insertions(+), 29 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index e161e793..4ae4fa73 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -129,7 +129,7 @@ def setStAbcParametershrp2017c(self): #abcp.default_zmp_offsets = [[0.015, -0.01, 0], [0.015, 0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.01, 0.01, 0], [0.01, -0.01, 0], [0, 0, 0], [0, 0, 0]] - # abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() @@ -234,10 +234,11 @@ def setStAbcParametershrp2016c (self): #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.010, 0.01, 0], [0.010, -0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.01, 0.035, 0], [0.01, -0.035, 0], [0, 0, 0], [0, 0, 0]] + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters - stp=self.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP + stp=self.abc_svc.getStabilizerParam() + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -253,7 +254,7 @@ def setStAbcParametershrp2016c (self): stp.eefm_pos_time_const_swing=0.08 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_zmp_delay_time_const=[0.0, 0.0] stp.eefm_cogvel_cutoff_freq=6.0 # mechanical foot edge #stp.eefm_leg_inside_margin=0.065 @@ -268,17 +269,17 @@ def setStAbcParametershrp2016c (self): stp.eefm_leg_outside_margin=tmp_leg_outside_margin stp.eefm_leg_front_margin=tmp_leg_front_margin stp.eefm_leg_rear_margin=tmp_leg_rear_margin - rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices 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_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] @@ -292,12 +293,14 @@ def setStAbcParametershrp2016c (self): stp.eefm_k2=[-0.36367379999999999, -0.36367379999999999] stp.eefm_k3=[-0.16200000000000001, -0.16200000000000001] # for estop - stp.emergency_check_mode=OpenHRP.StabilizerService.CP; + stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - self.st_svc.setParameter(stp) + stp.use_zmp_truncation = True + stp.detection_time_to_air = 1.0 + self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=1.1 @@ -312,12 +315,14 @@ def setStAbcParametershrp2016c (self): gg.swing_trajectory_final_distance_weight=1.5 gg.swing_trajectory_time_offset_xy2z=0.1 # [s] # - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*142.869; gg.heel_pos_offset_x = 1e-3*-105.784; gg.toe_zmp_offset_x = 1e-3*79.411; gg.heel_zmp_offset_x = 1e-3*-105.784; gg.use_toe_joint = True + gg.optional_go_pos_finalize_footstep_num = 1 + gg.overwritable_footstep_index_offset = 1 self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] @@ -331,10 +336,11 @@ def setStAbcParametershrp2007c (self): #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.010, 0.01, 0], [0.010, -0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.01, 0.035, 0], [0.01, -0.035, 0], [0, 0, 0], [0, 0, 0]] + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters - stp=self.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP + stp=self.abc_svc.getStabilizerParam() + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -365,17 +371,17 @@ def setStAbcParametershrp2007c (self): stp.eefm_leg_outside_margin=tmp_leg_outside_margin stp.eefm_leg_front_margin=tmp_leg_front_margin stp.eefm_leg_rear_margin=tmp_leg_rear_margin - rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices 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_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) # tpcc st params stp.k_tpcc_p=[2.0, 2.0] stp.k_tpcc_x=[5.0, 5.0] @@ -389,12 +395,14 @@ def setStAbcParametershrp2007c (self): stp.eefm_k2=[-0.36367379999999999, -0.36367379999999999] stp.eefm_k3=[-0.16200000000000001, -0.16200000000000001] # for estop - stp.emergency_check_mode=OpenHRP.StabilizerService.CP; + stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - self.st_svc.setParameter(stp) + stp.use_zmp_truncation = True + stp.detection_time_to_air = 1.0 + self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=1.1 @@ -414,6 +422,8 @@ def setStAbcParametershrp2007c (self): gg.heel_pos_offset_x = 1e-3*-106.925; gg.toe_zmp_offset_x = 1e-3*137.525; gg.heel_zmp_offset_x = 1e-3*-106.925; + gg.optional_go_pos_finalize_footstep_num = 1 + gg.overwritable_footstep_index_offset = 1 self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] From 252c9ea09dd81562aecf46d5dca43f54f548faed Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 30 Nov 2020 22:10:57 +0900 Subject: [PATCH 48/83] up --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 4ae4fa73..32f377e3 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -218,7 +218,7 @@ def setStAbcParametershrp2017c(self): gg.heel_pos_offset_x = 1e-3*-105.784; gg.toe_zmp_offset_x = 1e-3*79.411; gg.heel_zmp_offset_x = 1e-3*-105.784; - gg.use_toe_joint = True + gg.use_toe_joint = False gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 self.abc_svc.setGaitGeneratorParam(gg) @@ -320,7 +320,7 @@ def setStAbcParametershrp2016c (self): gg.heel_pos_offset_x = 1e-3*-105.784; gg.toe_zmp_offset_x = 1e-3*79.411; gg.heel_zmp_offset_x = 1e-3*-105.784; - gg.use_toe_joint = True + gg.use_toe_joint = False gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 self.abc_svc.setGaitGeneratorParam(gg) From 431648fe32fc47c5d5d4d09c923631b57231dfcc Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 11:34:13 +0900 Subject: [PATCH 49/83] up --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 32f377e3..b701f119 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -238,7 +238,7 @@ def setStAbcParametershrp2016c (self): self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -356,7 +356,7 @@ def setStAbcParametershrp2007c (self): stp.eefm_pos_time_const_swing=0.08 stp.eefm_pos_transition_time=0.01 stp.eefm_pos_margin_time=0.02 - stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_zmp_delay_time_const=[0.0, 0.0] stp.eefm_cogvel_cutoff_freq=6.0 # mechanical foot edge #stp.eefm_leg_inside_margin=0.07 @@ -417,7 +417,7 @@ def setStAbcParametershrp2007c (self): gg.swing_trajectory_final_distance_weight=1.5 gg.swing_trajectory_time_offset_xy2z=0.1 # [s] # - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*137.525; gg.heel_pos_offset_x = 1e-3*-106.925; gg.toe_zmp_offset_x = 1e-3*137.525; From 403458ea626aa64ce035fca873bf2f73f03b548b Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 13:27:29 +0900 Subject: [PATCH 50/83] add hrp2006 param --- .../hrp2_hrpsys_config.py | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index b701f119..db4091b6 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -405,10 +405,10 @@ def setStAbcParametershrp2007c (self): self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=1.1 - gg.default_double_support_ratio=0.32 + gg.default_step_time=0.8 + gg.default_double_support_ratio=0.1 #gg.swing_trajectory_delay_time_offset=0.35 - gg.swing_trajectory_delay_time_offset=0.2 + gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] # Orbit time parameters for delayhoffarbib (simultaneous xy and z landing) #gg.swing_trajectory_final_distance_weight=3.0 @@ -424,6 +424,20 @@ def setStAbcParametershrp2007c (self): gg.heel_zmp_offset_x = 1e-3*-106.925; gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 + gg.leg_marign = [0.13, 0.095, 0.062, 0.062] + gg.safe_leg_margin = [0.07, 0.06, 0.06, 0.06] + gg.stride_limitation_for_circle_type = [0.15, 0.25, 15, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] + gg.margin_time_ratio = 0.25 + gg.min_time_mgn = 0.3 + gg.use_disturbance_compensation = True + gg.dc_gain = 1e-3 + gg.modify_footsteps = True + gg.use_act_states = True + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.min_time = 0.7 + gg.overwritable_max_time = 1.0 + gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] From 2220b6097c1ab9ed1d7294a1ab31b2aaf6e061ec Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 13:50:20 +0900 Subject: [PATCH 51/83] add hrp2017 param --- .../hrp2_hrpsys_config.py | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index db4091b6..89df040a 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -200,10 +200,10 @@ def setStAbcParametershrp2017c(self): self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=1.1 - gg.default_double_support_ratio=0.32 + gg.default_step_time=0.8 + gg.default_double_support_ratio=0.1 #gg.swing_trajectory_delay_time_offset=0.35 - gg.swing_trajectory_delay_time_offset=0.2 + gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] # Orbit time parameters for delayhoffarbib (simultaneous xy and z landing) #gg.swing_trajectory_final_distance_weight=3.0 @@ -221,6 +221,20 @@ def setStAbcParametershrp2017c(self): gg.use_toe_joint = False gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 + gg.leg_marign = [0.13, 0.095, 0.062, 0.062] + gg.safe_leg_margin = [0.07, 0.06, 0.06, 0.06] + gg.stride_limitation_for_circle_type = [0.15, 0.25, 15, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] + gg.margin_time_ratio = 0.25 + gg.min_time_mgn = 0.3 + gg.use_disturbance_compensation = True + gg.dc_gain = 1e-3 + gg.modify_footsteps = True + gg.use_act_states = True + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.min_time = 0.7 + gg.overwritable_max_time = 1.0 + gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] From 3e60612b65bd7492aed38a9fce14e6a9e5e6eaa3 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 15:03:51 +0900 Subject: [PATCH 52/83] up --- .../hrp2_hrpsys_config.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 89df040a..3b265b87 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -193,8 +193,8 @@ def setStAbcParametershrp2017c(self): # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] + stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) @@ -222,18 +222,19 @@ def setStAbcParametershrp2017c(self): gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 gg.leg_marign = [0.13, 0.095, 0.062, 0.062] - gg.safe_leg_margin = [0.07, 0.06, 0.06, 0.06] - gg.stride_limitation_for_circle_type = [0.15, 0.25, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] + gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.35, 0.45, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 + gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE gg.min_time = 0.7 - gg.overwritable_max_time = 1.0 + gg.overwritable_max_time = 1.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop @@ -412,8 +413,8 @@ def setStAbcParametershrp2007c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] + stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) @@ -439,18 +440,19 @@ def setStAbcParametershrp2007c (self): gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 gg.leg_marign = [0.13, 0.095, 0.062, 0.062] - gg.safe_leg_margin = [0.07, 0.06, 0.06, 0.06] - gg.stride_limitation_for_circle_type = [0.15, 0.25, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] + gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.35, 0.45, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 + gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE gg.min_time = 0.7 - gg.overwritable_max_time = 1.0 + gg.overwritable_max_time = 1.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop From f930bc97c87bc6cacadc557fe05bd0b5a5b74efa Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 15:09:27 +0900 Subject: [PATCH 53/83] up --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3b265b87..44c3f0e4 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -193,8 +193,8 @@ def setStAbcParametershrp2017c(self): # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) @@ -413,8 +413,8 @@ def setStAbcParametershrp2007c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) From bd3a9a4625efe9386e8d5ba83805572334d95164 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 15:11:03 +0900 Subject: [PATCH 54/83] up --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 44c3f0e4..c0a3ba34 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -201,7 +201,7 @@ def setStAbcParametershrp2017c(self): # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=0.8 - gg.default_double_support_ratio=0.1 + gg.default_double_support_ratio=0.15 #gg.swing_trajectory_delay_time_offset=0.35 gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] @@ -421,7 +421,7 @@ def setStAbcParametershrp2007c (self): # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] gg.default_step_time=0.8 - gg.default_double_support_ratio=0.1 + gg.default_double_support_ratio=0.15 #gg.swing_trajectory_delay_time_offset=0.35 gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] From f17c6beae4a3ea0d0406794c2549030da6babbfd Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 1 Dec 2020 15:29:14 +0900 Subject: [PATCH 55/83] up --- .../hrp2_hrpsys_config.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index c0a3ba34..159a459c 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -196,7 +196,7 @@ def setStAbcParametershrp2017c(self): # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True - stp.detection_time_to_air = 1.0 + stp.detection_time_to_air = 2.0 self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] @@ -224,7 +224,7 @@ def setStAbcParametershrp2017c(self): gg.leg_marign = [0.13, 0.095, 0.062, 0.062] gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.45, 0, 0.35, 0.128] + gg.overwritable_stride_limitation = [0.35, 0.55, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True @@ -234,7 +234,7 @@ def setStAbcParametershrp2017c(self): gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE gg.min_time = 0.7 - gg.overwritable_max_time = 1.5 + gg.overwritable_max_time = 2.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop @@ -314,7 +314,7 @@ def setStAbcParametershrp2016c (self): stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True - stp.detection_time_to_air = 1.0 + stp.detection_time_to_air = 2.0 self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] @@ -442,7 +442,7 @@ def setStAbcParametershrp2007c (self): gg.leg_marign = [0.13, 0.095, 0.062, 0.062] gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.45, 0, 0.35, 0.128] + gg.overwritable_stride_limitation = [0.35, 0.55, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True @@ -452,7 +452,7 @@ def setStAbcParametershrp2007c (self): gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE gg.min_time = 0.7 - gg.overwritable_max_time = 1.5 + gg.overwritable_max_time = 2.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop From 57d0bec59d4bc099f23c49d415f257b0ca28f6bd Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 2 Dec 2020 14:37:17 +0900 Subject: [PATCH 56/83] up --- .../hrp2_hrpsys_config.py | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 159a459c..8dfc8d02 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -200,8 +200,8 @@ def setStAbcParametershrp2017c(self): self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=0.8 - gg.default_double_support_ratio=0.15 + gg.default_step_time=0.7 + gg.default_double_support_ratio=0.1 #gg.swing_trajectory_delay_time_offset=0.35 gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] @@ -221,20 +221,20 @@ def setStAbcParametershrp2017c(self): gg.use_toe_joint = False gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 - gg.leg_marign = [0.13, 0.095, 0.062, 0.062] + gg.leg_margin = [0.13, 0.095, 0.062, 0.062] gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.55, 0, 0.35, 0.128] + gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 - gg.dcm_offset = 0.02 + gg.dcm_offset = 0.0 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE - gg.min_time = 0.7 - gg.overwritable_max_time = 2.5 + gg.min_time = 0.6 + gg.overwritable_max_time = 1.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop @@ -420,8 +420,8 @@ def setStAbcParametershrp2007c (self): self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=0.8 - gg.default_double_support_ratio=0.15 + gg.default_step_time=0.7 + gg.default_double_support_ratio=0.1 #gg.swing_trajectory_delay_time_offset=0.35 gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] @@ -439,20 +439,20 @@ def setStAbcParametershrp2007c (self): gg.heel_zmp_offset_x = 1e-3*-106.925; gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 - gg.leg_marign = [0.13, 0.095, 0.062, 0.062] + gg.leg_margin = [0.13, 0.095, 0.062, 0.062] gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.55, 0, 0.35, 0.128] + gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] gg.margin_time_ratio = 0.25 gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 - gg.dcm_offset = 0.02 + gg.dcm_offset = 0.0 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE - gg.min_time = 0.7 - gg.overwritable_max_time = 2.5 + gg.min_time = 0.6 + gg.overwritable_max_time = 1.5 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop From bd818263b028d566d2f9c2dc67db7c52fd7ff55d Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 2 Dec 2020 15:51:23 +0900 Subject: [PATCH 57/83] up --- .../hrp2_hrpsys_config.py | 33 ++++++++++++++----- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 8dfc8d02..22a87582 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -129,7 +129,7 @@ def setStAbcParametershrp2017c(self): #abcp.default_zmp_offsets = [[0.015, -0.01, 0], [0.015, 0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.01, 0.01, 0], [0.01, -0.01, 0], [0, 0, 0], [0, 0, 0]] - abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY + # abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() @@ -249,7 +249,7 @@ def setStAbcParametershrp2016c (self): #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.010, 0.01, 0], [0.010, -0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.01, 0.035, 0], [0.01, -0.035, 0], [0, 0, 0], [0, 0, 0]] - abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY + # abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() @@ -311,17 +311,17 @@ def setStAbcParametershrp2016c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True - stp.detection_time_to_air = 2.0 + stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=1.1 - gg.default_double_support_ratio=0.32 + gg.default_step_time=0.7 + gg.default_double_support_ratio=0.1 #gg.swing_trajectory_delay_time_offset=0.35 - gg.swing_trajectory_delay_time_offset=0.2 + gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] # Orbit time parameters for delayhoffarbib (simultaneous xy and z landing) #gg.swing_trajectory_final_distance_weight=3.0 @@ -338,6 +338,21 @@ def setStAbcParametershrp2016c (self): gg.use_toe_joint = False gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 + gg.leg_margin = [0.13, 0.095, 0.062, 0.062] + gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] + gg.margin_time_ratio = 0.25 + gg.min_time_mgn = 0.3 + gg.use_disturbance_compensation = True + gg.dc_gain = 1e-3 + gg.dcm_offset = 0.0 + gg.modify_footsteps = True + gg.use_act_states = True + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.min_time = 0.6 + gg.overwritable_max_time = 1.5 + gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] @@ -351,7 +366,7 @@ def setStAbcParametershrp2007c (self): #abcp.default_zmp_offsets = [[0.015, 0.01, 0], [0.015, -0.01, 0], [0, 0, 0], [0, 0, 0]] abcp.default_zmp_offsets = [[0.010, 0.01, 0], [0.010, -0.01, 0], [0, 0, 0], [0, 0, 0]] #abcp.default_zmp_offsets = [[0.01, 0.035, 0], [0.01, -0.035, 0], [0, 0, 0], [0, 0, 0]] - abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY + # abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() From c739a250396ff9fab5278378f4695777d8b78684 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Wed, 2 Dec 2020 16:24:36 +0900 Subject: [PATCH 58/83] fix typo --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 22a87582..120e5d35 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -135,7 +135,7 @@ def setStAbcParametershrp2017c(self): stp=self.abc_svc.getStabilizerParam() stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP # eefm st params - stp.eefm_body_attitude_control_again=[1.5, 1.5] + stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] # EEFM parameters for 4 limbs #stp.eefm_rot_damping_gain = [[20*1.6, 20*1.6, 1e5]]*4 From 2f6e6e2af9432169516b2dca9901eda6712ea3fd Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 2 Dec 2020 17:26:26 +0900 Subject: [PATCH 59/83] up --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 120e5d35..0ae59ba5 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -229,7 +229,7 @@ def setStAbcParametershrp2017c(self): gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 - gg.dcm_offset = 0.0 + gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE @@ -346,7 +346,7 @@ def setStAbcParametershrp2016c (self): gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 - gg.dcm_offset = 0.0 + gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE @@ -462,7 +462,7 @@ def setStAbcParametershrp2007c (self): gg.min_time_mgn = 0.3 gg.use_disturbance_compensation = True gg.dc_gain = 1e-3 - gg.dcm_offset = 0.0 + gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE From 92e17147b20bdad516174150d911279109a7cc6e Mon Sep 17 00:00:00 2001 From: HRP2 Date: Sat, 5 Dec 2020 13:46:49 +0900 Subject: [PATCH 60/83] up --- .../hrp2_hrpsys_config.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 0ae59ba5..c30b48a5 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -133,7 +133,7 @@ def setStAbcParametershrp2017c(self): self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -193,8 +193,8 @@ def setStAbcParametershrp2017c(self): # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 2.0 self.abc_svc.setStabilizerParam(stp) @@ -253,7 +253,7 @@ def setStAbcParametershrp2016c (self): self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -311,8 +311,8 @@ def setStAbcParametershrp2016c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - # stp.eefm_swing_pos_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] - # stp.eefm_swing_rot_spring_gain = [[1]*3, [1]*3, [0]*3, [0]*3] + stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) @@ -370,7 +370,7 @@ def setStAbcParametershrp2007c (self): self.abc_svc.setAutoBalancerParam(abcp) # ST parameters stp=self.abc_svc.getStabilizerParam() - stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -428,8 +428,8 @@ def setStAbcParametershrp2007c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) From 2349d601dc4aac369532737431542bf696ec96f6 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sat, 5 Dec 2020 14:51:36 +0900 Subject: [PATCH 61/83] up --- .../hrp2_hrpsys_config.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index c30b48a5..95f59790 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -193,8 +193,8 @@ def setStAbcParametershrp2017c(self): # stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 2.0 self.abc_svc.setStabilizerParam(stp) @@ -311,8 +311,8 @@ def setStAbcParametershrp2016c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) @@ -428,8 +428,8 @@ def setStAbcParametershrp2007c (self): stp.emergency_check_mode=OpenHRP.AutoBalancerService.CP; stp.cp_check_margin=[50*1e-3, 45*1e-3, 0, 100*1e-3]; # for swing - stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] - stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_pos_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] + # stp.eefm_swing_rot_spring_gain = [[5]*3, [5]*3, [0]*3, [0]*3] stp.use_zmp_truncation = True stp.detection_time_to_air = 1.0 self.abc_svc.setStabilizerParam(stp) From c255741a604306cff26518a6781fc8acd4c9631b Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sat, 5 Dec 2020 15:12:38 +0900 Subject: [PATCH 62/83] remove octd rfu --- .../src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 95f59790..e4f5850b 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -41,9 +41,9 @@ def getRTCList(self): ['kf', "KalmanFilter"], #['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], - ['octd', "ObjectContactTurnaroundDetector"], + # ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], - ['rfu', "ReferenceForceUpdater"], + # ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], # ['st', "Stabilizer"], From e1e278226c967275ac481a0dbed7367eab3778b3 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 7 Dec 2020 11:51:30 +0900 Subject: [PATCH 63/83] [hrp2_hrpsys_config] fix st/abc parameter for real robot --- .../hrp2_hrpsys_config.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index e4f5850b..aabe80cd 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -200,8 +200,8 @@ def setStAbcParametershrp2017c(self): self.abc_svc.setStabilizerParam(stp) # GG parameters gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=0.7 - gg.default_double_support_ratio=0.1 + gg.default_step_time=1.0 + gg.default_double_support_ratio=0.2 #gg.swing_trajectory_delay_time_offset=0.35 gg.swing_trajectory_delay_time_offset= gg.default_step_time * (1.0 - gg.default_double_support_ratio) * 0.3 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] @@ -222,19 +222,19 @@ def setStAbcParametershrp2017c(self): gg.optional_go_pos_finalize_footstep_num = 1 gg.overwritable_footstep_index_offset = 1 gg.leg_margin = [0.13, 0.095, 0.062, 0.062] - gg.safe_leg_margin = [0.07, 0.055, 0.057, 0.057] - gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.1, 0.138] - gg.overwritable_stride_limitation = [0.35, 0.5, 0, 0.35, 0.128] - gg.margin_time_ratio = 0.25 + gg.safe_leg_margin = [0.06, 0.05, 0.062, 0.05] + gg.stride_limitation_for_circle_type = [0.15, 0.3, 10, 0.1, 0.138] + gg.overwritable_stride_limitation = [0.25, 0.35, 0, 0.25, 0.128] + gg.margin_time_ratio = 0.3 gg.min_time_mgn = 0.3 - gg.use_disturbance_compensation = True + gg.use_disturbance_compensation = False gg.dc_gain = 1e-3 gg.dcm_offset = 0.02 gg.modify_footsteps = True gg.use_act_states = True gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE - gg.min_time = 0.6 - gg.overwritable_max_time = 1.5 + gg.min_time = 1.0 + gg.overwritable_max_time = 2.0 gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop From 142878410acb2aadcae84788076aea5b439bdf57 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 7 Dec 2020 11:55:50 +0900 Subject: [PATCH 64/83] [HRP2JSK*] add reset-manip-walk-pose --- hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml | 10 ++++++++++ hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml | 10 ++++++++++ .../models/hrp2jsknts.yaml | 10 ++++++++++ .../hrp2_hrpsys_config.py | 15 +++++++++++++++ 4 files changed, 45 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml index b1cd69da..4af9ecef 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsk.yaml @@ -98,6 +98,16 @@ angle-vector: 0.0, 40.0, 50.0, -30.0, -10.0, -120.0, -25.0, -5.0, -20.0, 60.0, 50.0, 30.0, 10.0, -120.0, 25.0, 5.0, -20.0, -60.0] + # (send *robot* :reset-manip-pose) + # (send *robot* :arms :move-end-pos #f(50 0 0)) + # (send *robot* :legs :move-end-pos #f(0 0 20)) + # (map cons #'(lambda (q) (format t "~A, " q)) (send *robot* :angle-vector)) + reset-manip-walk-pose: [-9.115767e-06, -0.000114, -30.2316, 58.3867, -28.1551, 0.000136, + 9.115767e-06, 0.000114, -30.2316, 58.3867, -28.1551, -0.000136, + 0.0, 0.0, + 0.0, 40.0, + 35.6581, -31.9516, -5.0731, -115.719, -25.5902, -6.6445, -11.7106, 60.0, + 35.6581, 31.9516, 5.0731, -115.719, 25.5902, 6.6445, -11.7106, -60.0] # for gazebo simulation replace_xmls: diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml index fd338a87..e87aa13b 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml @@ -147,6 +147,16 @@ angle-vector: 0.0, 40.0, 50.0, -30.0, -10.0, -120.0, -25.0, -5.0, -20.0, 60.0, 50.0, 30.0, 10.0, -120.0, 25.0, 5.0, -20.0, -60.0] + # (send *robot* :reset-manip-pose) + # (send *robot* :arms :move-end-pos #f(50 0 0)) + # (send *robot* :legs :move-end-pos #f(0 0 20)) + # (map cons #'(lambda (q) (format t "~A, " q)) (send *robot* :angle-vector)) + reset-manip-walk-pose: [-2.854655e-06, -4.435380e-05, -30.2315, 58.3865, -28.155, 5.273264e-05, 0.0, + 2.854655e-06, 4.435380e-05, -30.2315, 58.3865, -28.155, -5.273264e-05, 0.0, + 0.0, 0.0, + 0.0, 40.0, + 35.6582, -31.9516, -5.07314, -115.72, -25.5902, -6.64447, -11.7105, 60.0, + 35.6582, 31.9516, 5.07314, -115.72, 25.5902, 6.64447, -11.7105, -60.0] # for gazebo simulation replace_xmls: - match_rule: diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml index fa043925..5d60a1c9 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml @@ -153,6 +153,16 @@ angle-vector: 0.0, 40.0, 50.0, -30.0, -10.0, -120.0, -25.0, -5.0, -20.0, 60.0, 50.0, 30.0, 10.0, -120.0, 25.0, 5.0, -20.0, -60.0] + # (send *robot* :reset-manip-pose) + # (send *robot* :arms :move-end-pos #f(50 0 0)) + # (send *robot* :legs :move-end-pos #f(0 0 20)) + # (map cons #'(lambda (q) (format t "~A, " q)) (send *robot* :angle-vector)) + reset-manip-walk-pose: [-2.854655e-06, -4.435380e-05, -30.2315, 58.3865, -28.155, 5.273264e-05, 0.0, + 2.854655e-06, 4.435380e-05, -30.2315, 58.3865, -28.155, -5.273264e-05, 0.0, + 0.0, 0.0, + 0.0, 40.0, + 35.6582, -31.9516, -5.07314, -115.72, -25.5902, -6.64447, -11.7105, 60.0, + 35.6582, 31.9516, 5.07314, -115.72, 25.5902, 6.64447, -11.7105, -60.0] # for gazebo simulation replace_xmls: - match_rule: diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index e4f5850b..275c02ba 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py @@ -103,6 +103,18 @@ def hrp2ResetManipPose (self): else: return [0.0, 0.0, 0.0, 0.698132, 0.872665, -0.523599, -0.174533, -2.0944, -0.436332, -0.087266, -0.349066, 1.0472, 0.872665, 0.523599, 0.174533, -2.0944, 0.436332, 0.087266, -0.349066, -1.0472] + def hrp2ResetManipWalkPose (self): + # (send *robot* :reset-manip-pose) + # (send *robot* :arms :move-end-pos #f(50 0 0)) + # (send *robot* :legs :move-end-pos #f(0 0 20)) + # (map cons #'(lambda (q) (format t "~A, " (deg2rad q))) (send *robot* :angle-vector)) + if self.ROBOT_NAME.find("HRP2JSKNT") != -1: + return [-4.982313e-08, -7.741199e-07, -0.527639, 1.01904, -0.491398, 9.203582e-07, 0.0, 4.982313e-08, 7.741199e-07, -0.527639, 1.01904, -0.491398, -9.203582e-07, 0.0, 0.0, 0.0, 0.0, 0.698132, 0.622352, -0.557661, -0.088543, -2.01969, -0.446634, -0.115968, -0.204386, 1.0472, 0.622352, 0.557661, 0.088543, -2.01969, 0.446634, 0.115968, -0.204386, -1.0472] + elif self.ROBOT_NAME.find("HRP2JSK") != -1: + return [-1.591002e-07, -1.988452e-06, -0.527641, 1.01904, -0.4914, 2.368305e-06, 1.591002e-07, 1.988452e-06, -0.527641, 1.01904, -0.4914, -2.368305e-06, 0.0, 0.0, 0.0, 0.698132, 0.622352, -0.55766, -0.088542, -2.01969, -0.446633, -0.115968, -0.204388, 1.0472, 0.622352, 0.55766, 0.088542, -2.01969, 0.446633, 0.115968, -0.204388, -1.0472] + else: + return [0.0, 0.0, 0.0, 0.698132, 0.872665, -0.523599, -0.174533, -2.0944, -0.436332, -0.087266, -0.349066, 1.0472, 0.872665, 0.523599, 0.174533, -2.0944, 0.436332, 0.087266, -0.349066, -1.0472] + def hrp2InitPose (self): if self.ROBOT_NAME.find("HRP2JSKNT") != -1: return [0]*len(self.hrp2ResetPose()) @@ -482,6 +494,9 @@ def setResetPose(self): def setResetManipPose(self): self.seq_svc.setJointAngles(self.hrp2ResetManipPose(), 5.0) + def setResetManipWalkPose(self): + self.seq_svc.setJointAngles(self.hrp2ResetManipWalkPose(), 5.0) + def setInitPose(self): self.seq_svc.setJointAngles(self.hrp2InitPose(), 5.0) From cb65d9eec33ee596803b8bff01eff1823915f65a Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Fri, 18 Dec 2020 20:11:33 +0900 Subject: [PATCH 65/83] up --- .../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 32cb547b..bb87985b 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 @@ -295,9 +295,9 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): # "landing_pgain":[5,30,5,1,0.1,0.1], "landing_dgain":[70,70,50,10,0.1,0.1], # "swing_pgain":[5,30,10,5,10,10], "swing_dgain":[70,70,50,10,10,10]} # # "swing_pgain":[5,30,10,5,0.15,0.12], "swing_dgain":[70,70,50,10,0.1,0.1]} - leg_gains = {"support_pgain":[5,5,5,5,0.1,0.1], "support_dgain":[70,20,20,20,0.1,0.1], - "landing_pgain":[5,1,1,1,0.1,0.1], "landing_dgain":[70,10,10,10,0.1,0.1], - "swing_pgain":[5,30,10,10,10,10], "swing_dgain":[70,70,50,50,10,10]} + leg_gains = {"support_pgain":[5,10,10,5,0.1,0.1], "support_dgain":[10,20,20,10,10,10], + "landing_pgain":[5,1,1,1,0.1,0.1], "landing_dgain":[10,10,10,10,5,5], + "swing_pgain":[5,30,20,10,5,5], "swing_dgain":[10,30,20,20,10,10]} arm_gains = {"support_pgain":[100,100,100,100,100,100,100,100], "support_dgain":[100,100,100,100,100,100,100,100], "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} From 3695706eba188491a0d93395a2cd75d8bb9686a4 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Fri, 2 Apr 2021 21:41:01 +0900 Subject: [PATCH 66/83] up --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 5 +++-- 1 file changed, 3 insertions(+), 2 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 bb87985b..8232ff96 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 @@ -237,8 +237,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=False # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0]]*4 - astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0]]*4 + astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] @@ -437,6 +437,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; gg.optional_go_pos_finalize_footstep_num=1 + gg.front_edge_offset_of_steppable_region = [-0.02, 0.0] self.abc_svc.setGaitGeneratorParam(gg) # Ic setting limbs = ['rarm', 'larm'] From 72a48a48b33077df70cf7b79864ce9ad8b9ac1d3 Mon Sep 17 00:00:00 2001 From: Keitaro Murakami Date: Tue, 22 Jun 2021 20:02:00 +0900 Subject: [PATCH 67/83] set default value of retrieve duration --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 ++ 1 file changed, 2 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 8232ff96..25a04c91 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 @@ -343,6 +343,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): esp=self.es_svc.getEmergencyStopperParam()[1] esp.default_recover_time=3.0 # [s] esp.default_retrieve_time=1.0 # [s] + esp.default_retrieve_duration=1.0 # [s] self.es_svc.setEmergencyStopperParam(esp) def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): @@ -450,6 +451,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): esp=self.es_svc.getEmergencyStopperParam()[1] esp.default_recover_time=10.0 # [s] esp.default_retrieve_time=1.0 # [s] + esp.default_retrieve_duration=1.0 # [s] self.es_svc.setEmergencyStopperParam(esp) def setStAbcParametersURATALEG (self): From a8dfc508e52bdcd56eee44b04b93a462712d650b Mon Sep 17 00:00:00 2001 From: kirohy Date: Mon, 21 Jun 2021 20:35:49 +0900 Subject: [PATCH 68/83] [hrpsys_ros_bridge_tutorials][JAXON_RED] replace foot with Leptrino force sensor --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 4 +++- hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml | 4 ++-- .../urata_hrpsys_config.py | 12 +++++++++--- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 52d03bd9..dd381fa5 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -382,10 +382,12 @@ compile_rbrain_model_for_closed_robots(JAXON_RED JAXON_RED JAXON_RED --simulation-timestep-option "0.002" --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0" + ## Leptrino force sensor foot with sponge sole (10mm) + --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1155,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1155,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0,1.0,0.0,1.5708," ## 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_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,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.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,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.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,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_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT7,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_red.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml index b79c6a8b..9da0a9b1 100644 --- a/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml +++ b/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml @@ -135,8 +135,8 @@ sensors: # - {sensor_name: 'gsensor', sensor_type: 'acceleration', parent_link: 'BODY', translate: '0 0 0', rotate: '0 0 1 180'} # - {sensor_name: 'gyrometer', sensor_type: 'gyro', parent_link: 'BODY', translate: '0 0 0', rotate: '0 0 1 180'} ### sensor position should be confirmed - - {sensor_name: 'rfsensor', sensor_type: 'base_force6d', parent_link: 'RLEG_LINK5', translate: '0 0 -0.069', rotate: '0 1 0 180'} - - {sensor_name: 'lfsensor', sensor_type: 'base_force6d', parent_link: 'LLEG_LINK5', translate: '0 0 -0.069', rotate: '0 1 0 180'} + - {sensor_name: 'rfsensor', sensor_type: 'base_force6d', parent_link: 'RLEG_LINK5', translate: '0 0 -0.0825', rotate: '1 0 0 180'} + - {sensor_name: 'lfsensor', sensor_type: 'base_force6d', parent_link: 'LLEG_LINK5', translate: '0 0 -0.0825', rotate: '0 1 0 180'} - {sensor_name: 'rhsensor', sensor_type: 'base_force6d', parent_link: 'RARM_LINK7', translate: '0 0 -0.069', rotate: '-0.382683 -0.92388 0 180'} - {sensor_name: 'lhsensor', sensor_type: 'base_force6d', parent_link: 'LARM_LINK7', translate: '0 0 -0.069', rotate: '0.382683 -0.92388 0 180'} - {sensor_name: 'fisheye', sensor_type: 'camera', parent_link: 'CHEST_LINK2', translate: '0.147 0 -0.022', rotate: '-0.694747 0.694747 -0.186157 158.909'} 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 25a04c91..486b5307 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 @@ -62,9 +62,9 @@ def setStAbcParameters (self): if self.ROBOT_NAME == "STARO": self.setStAbcParametersSTARO() elif self.ROBOT_NAME == "JAXON": - self.setStAbcIcParametersJAXON(foot="LEPTRINO") + self.setStAbcIcParametersJAXON(foot="LEPTRINO_FORCE_PLATE") elif self.ROBOT_NAME == "JAXON_RED": - self.setStAbcIcParametersJAXON(foot="KAWADA") + self.setStAbcIcParametersJAXON(foot="LEPTRINO_FORCE_SENSOR") elif self.ROBOT_NAME == "JAXON_BLUE": self.setStAbcIcParametersJAXON_BLUE() elif self.ROBOT_NAME == "URATALEG": @@ -266,11 +266,17 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_leg_outside_margin=0.07 astp.eefm_leg_front_margin=0.1 astp.eefm_leg_rear_margin=0.1 - elif foot == "LEPTRINO": + elif foot == "LEPTRINO_FORCE_PLATE": astp.eefm_leg_inside_margin=0.065 astp.eefm_leg_outside_margin=0.065 astp.eefm_leg_front_margin=0.115 astp.eefm_leg_rear_margin=0.115 + elif foot == "LEPTRINO_FORCE_SENSOR": + ## Leptrino force sensor foot : mechanical param is => inside 0.07, front 0.12, rear 0.11 + astp.eefm_leg_inside_margin=0.065 + astp.eefm_leg_outside_margin=0.065 + astp.eefm_leg_front_margin=0.115 + astp.eefm_leg_rear_margin=0.105 rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_inside_margin]), OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, -1*astp.eefm_leg_outside_margin]), OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, -1*astp.eefm_leg_outside_margin]), From 5cd85b000d8117431fd03b5c05762608d686882b Mon Sep 17 00:00:00 2001 From: kirohy Date: Mon, 21 Jun 2021 20:38:41 +0900 Subject: [PATCH 69/83] [hrpsys_ros_bridge_tutorials][JAXON] replace foot with KAWADA --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 4 ++-- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index dd381fa5..14aac906 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -358,13 +358,13 @@ compile_rbrain_model_for_closed_robots(JAXON JAXON JAXON --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0" ## Leptrino foot - --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,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.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," ## 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_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708," ## KAWADA foot + hand contact end-coords --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_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot + hand contact end-coords" ## KAWADA foot with rubber shoes - --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708," + --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708," ## JSK foot --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_JOINT7,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,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 RLEG_JOINT2:LARM_JOINT7 RLEG_JOINT3:LARM_JOINT7 RLEG_JOINT5:LARM_JOINT7 LLEG_JOINT2:LARM_JOINT7 LLEG_JOINT3:LARM_JOINT7 LLEG_JOINT5:LARM_JOINT7 CHEST_JOINT1:LARM_JOINT7 HEAD_JOINT1:LARM_JOINT7 RARM_JOINT0:LARM_JOINT7 RARM_JOINT2:LARM_JOINT7 RARM_JOINT3:LARM_JOINT7 RARM_JOINT4:LARM_JOINT7 RARM_JOINT5:LARM_JOINT7 RARM_JOINT7:LARM_JOINT7 LARM_JOINT7:WAIST RLEG_JOINT2:RARM_JOINT7 RLEG_JOINT3:RARM_JOINT7 RLEG_JOINT5:RARM_JOINT7 LLEG_JOINT2:RARM_JOINT7 LLEG_JOINT3:RARM_JOINT7 LLEG_JOINT5:RARM_JOINT7 CHEST_JOINT1:RARM_JOINT7 HEAD_JOINT1:RARM_JOINT7 RARM_JOINT7:LARM_JOINT0 RARM_JOINT7:LARM_JOINT2 RARM_JOINT7:LARM_JOINT3 RARM_JOINT7:LARM_JOINT4 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:RARM_JOINT7 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT7 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2" 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 486b5307..fba344f8 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 @@ -62,7 +62,7 @@ def setStAbcParameters (self): if self.ROBOT_NAME == "STARO": self.setStAbcParametersSTARO() elif self.ROBOT_NAME == "JAXON": - self.setStAbcIcParametersJAXON(foot="LEPTRINO_FORCE_PLATE") + self.setStAbcIcParametersJAXON(foot="KAWADA") elif self.ROBOT_NAME == "JAXON_RED": self.setStAbcIcParametersJAXON(foot="LEPTRINO_FORCE_SENSOR") elif self.ROBOT_NAME == "JAXON_BLUE": From 7a23d48a82c450dfee405ccf7cee267a0cbb8da9 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sun, 4 Jul 2021 12:06:22 +0900 Subject: [PATCH 70/83] up --- .../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 25a04c91..0d14b461 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 @@ -182,7 +182,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): if self.ROBOT_NAME == "JAXON": abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; elif self.ROBOT_NAME == "JAXON_RED": - abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -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]]; # TODO: ここでセットしたzmp_offsetsが反映されないことがある abcp.move_base_gain=1.0 abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) From 763a6c639338e3efe55db1a10db1e9332ee189e0 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Fri, 16 Jul 2021 16:44:37 +0900 Subject: [PATCH 71/83] up --- .../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 0d14b461..dfa12b90 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 @@ -182,7 +182,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): if self.ROBOT_NAME == "JAXON": abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; elif self.ROBOT_NAME == "JAXON_RED": - abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; # TODO: ここでセットしたzmp_offsetsが反映されないことがある + abcp.default_zmp_offsets=[[0.0, 0.02, 0.0], [0.0, -0.02, 0.0], [0, 0, 0], [0, 0, 0]]; # TODO: zmp_offsets here is sometimes not set abcp.move_base_gain=1.0 abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) From 9cfda20f149b6d7fa21662d0773ec109d939bb9f Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sun, 18 Jul 2021 19:45:59 +0900 Subject: [PATCH 72/83] rm sernsorRPY_offset --- .travis | 2 +- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis b/.travis index 0955a49e..74e96892 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 0955a49eb558dbcc8f25c032668ad25774c41a8b +Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 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 29808ef4..6c6ec7b2 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 @@ -197,7 +197,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] - kfp.sensorRPY_offset = [-0.015, 0.022, 0] + # kfp.sensorRPY_offset = [-0.015, 0.022, 0] kfp.R_angle=1000 self.kf_svc.setKalmanFilterParam(kfp) # AutoSt setting From 30a574208cb95faef2fd9821081c16466b023925 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Thu, 11 Nov 2021 14:32:30 +0900 Subject: [PATCH 73/83] up --- .../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 6c6ec7b2..00493894 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 @@ -341,10 +341,10 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] gg.swing_trajectory_final_distance_weight=3.0 gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE - gg.toe_pos_offset_x = 1e-3*117.338; - gg.heel_pos_offset_x = 1e-3*-116.342; - gg.toe_zmp_offset_x = 1e-3*117.338; - gg.heel_zmp_offset_x = 1e-3*-116.342; + gg.toe_pos_offset_x = 1e-3*115.0; + gg.heel_pos_offset_x = 1e-3*-115.0; + gg.toe_zmp_offset_x = 1e-3*115.0; + gg.heel_zmp_offset_x = 1e-3*-115.0; gg.optional_go_pos_finalize_footstep_num=1 gg.overwritable_footstep_index_offset=1 self.abc_svc.setGaitGeneratorParam(gg) From 2fba4702855df9e983bb1d283e63f8201915469f Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 17 Nov 2021 14:45:28 +0900 Subject: [PATCH 74/83] up --- .../src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 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 00493894..3875689c 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 @@ -242,10 +242,10 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] astp.eefm_zmp_delay_time_const=[0, 0] astp.detection_time_to_air=5.0 - astp.use_zmp_truncation=True + astp.use_zmp_truncation=False astp.eefm_swing_damping_force_thre=[200]*3 astp.eefm_swing_damping_moment_thre=[15]*3 - astp.eefm_use_swing_damping=False + astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] From 6400605a1e2238d1819c302a340d03bffd9a7324 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 17 Nov 2021 20:45:09 +0900 Subject: [PATCH 75/83] disable rot spring gain --- .../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 3875689c..69c8a5f1 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 @@ -247,7 +247,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 From 696a8708732d7e325b7f27633b02ced04f3dd683 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Fri, 19 Nov 2021 23:41:01 +0900 Subject: [PATCH 76/83] up --- .../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 69c8a5f1..21a90ce9 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 @@ -247,7 +247,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - # astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_spring_gain=[[0.5, 0.5, 0.5], [0.5, 0.5, 0.5], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 From 7a805c148a0572a60fe549e0745a4b464fcb5a5e Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sat, 4 Dec 2021 22:40:55 +0900 Subject: [PATCH 77/83] up --- .../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 21a90ce9..d84de6d6 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 @@ -242,7 +242,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_pos_compensation_limit = [0.08, 0.08, 0.050, 0.050] astp.eefm_zmp_delay_time_const=[0, 0] astp.detection_time_to_air=5.0 - astp.use_zmp_truncation=False + astp.use_zmp_truncation=True astp.eefm_swing_damping_force_thre=[200]*3 astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True From c1bc9f978dad17f6879b83a2e895db0cedd70b76 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sun, 2 Jan 2022 20:57:22 +0900 Subject: [PATCH 78/83] up --- .../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 d84de6d6..59af6438 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 @@ -247,7 +247,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[0.5, 0.5, 0.5], [0.5, 0.5, 0.5], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 From cae7c99395aacacfe288b6b603088c6456d898e0 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sun, 13 Feb 2022 19:04:03 +0900 Subject: [PATCH 79/83] up --- .../urata_hrpsys_config.py | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 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 59af6438..53839ea8 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 @@ -329,15 +329,13 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): #gg.default_step_time=1.0 #self.abc_svc.setGaitGeneratorParam(gg) gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=1.2 - gg.default_step_height=0.065 + gg.default_step_time=0.8 + gg.default_step_height=0.07 #gg.default_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 + gg.default_double_support_ratio=0.15 #gg.stride_parameter=[0.1,0.05,10.0] #gg.default_step_time=1.0 - #gg.swing_trajectory_delay_time_offset=0.35 - #gg.swing_trajectory_delay_time_offset=0.2 - gg.swing_trajectory_delay_time_offset=0.15 + gg.swing_trajectory_delay_time_offset=0.238 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] gg.swing_trajectory_final_distance_weight=3.0 gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE @@ -347,6 +345,20 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): gg.heel_zmp_offset_x = 1e-3*-115.0; gg.optional_go_pos_finalize_footstep_num=1 gg.overwritable_footstep_index_offset=1 + gg.leg_margin = [0.115, 0.115, 0.065, 0.065] + gg.safe_leg_margin = [0.075, 0.075, 0.065, 0.065] + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.15, 0.135] + gg.overwritable_stride_limitation = [0.3, 0.45, 0, 0.3, 0.125] + gg.margin_time_ratio = 0.0 + gg.min_time_margin = 0.12 + gg.min_time = 0.7 + gg.overwritable_max_time = 1.0 + gg.modify_footsteps = True + gg.use_act_states = True + gg.emergency_step_time = [0.02, 0.6, 0.7] + gg.rectangle_goal_off = [0, 0, -0.05] + gg.dcm_offset = 0.01 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting limbs = ['rarm', 'larm'] From 16cef356bc911df8ffe827f9503d065733198a5a Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 15 Feb 2022 17:09:27 +0900 Subject: [PATCH 80/83] add jaxon_blue --- .../urata_hrpsys_config.py | 153 +++++++++++------- 1 file changed, 93 insertions(+), 60 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 53839ea8..e54a7ac4 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 @@ -377,96 +377,129 @@ 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 + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY self.abc_svc.setAutoBalancerParam(abcp) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] kfp.R_angle=1000 self.kf_svc.setKalmanFilterParam(kfp) - # st setting - stp=self.st_svc.getParameter() - #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 - stp.cp_check_margin=[0.05, 0.045, 0, 0.095] - stp.k_brot_p=[0, 0] - stp.k_brot_tc=[1000, 1000] - #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 = [[25, 25, 1e5], # modification with kojio + # AutoSt setting + astp=self.abc_svc.getStabilizerParam() + #astp.st_algorithm=OpenHRP.AutoBalancerService.EEFM + astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP + # astp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQPCOP + astp.emergency_check_mode=OpenHRP.AutoBalancerService.CP # enable EmergencyStopper for JAXON @ 2015/11/19 + astp.cp_check_margin=[0.05, 0.045, 0, 0.095] + astp.k_brot_p=[0, 0] + astp.k_brot_tc=[1000, 1000] + #astp.eefm_body_attitude_control_gain=[0, 0.5] + astp.eefm_body_attitude_control_gain=[0.5, 0.5] + astp.eefm_body_attitude_control_time_const=[1000, 1000] + astp.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? + astp.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_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 - stp.eefm_wrench_alpha_blending=0.7 - stp.eefm_pos_time_const_swing=0.06 - stp.eefm_pos_transition_time=0.01 - stp.eefm_pos_margin_time=0.02 + astp.eefm_swing_pos_damping_gain = astp.eefm_pos_damping_gain[0] # same with support leg + astp.eefm_swing_rot_damping_gain = astp.eefm_rot_damping_gain[0] # same with support leg + astp.eefm_rot_compensation_limit = [math.radians(30), math.radians(30), math.radians(10), math.radians(10)] + astp.eefm_pos_compensation_limit = [0.050, 0.050, 0.050, 0.050] + astp.eefm_zmp_delay_time_const=[0, 0] + astp.detection_time_to_air=5.0 + astp.use_zmp_truncation=True + astp.eefm_swing_damping_force_thre=[200]*3 + astp.eefm_swing_damping_moment_thre=[15]*3 + astp.eefm_use_swing_damping=True + # astp.eefm_ee_error_cutoff_freq=10000 # not used + astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 + astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 + astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] + astp.eefm_rot_time_const = [[1.5/1.1, 1.5/1.1, 1.5/1.1]]*4 + astp.eefm_pos_time_const_support = [[3.0/1.1, 3.0/1.1, 1.5/1.1]]*4 + astp.eefm_wrench_alpha_blending=0.7 + astp.eefm_pos_time_const_swing=0.06 + astp.eefm_pos_transition_time=0.01 + astp.eefm_pos_margin_time=0.02 # foot margin param - stp.eefm_leg_inside_margin=0.05 - stp.eefm_leg_outside_margin=0.05 - 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]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, stp.eefm_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[stp.eefm_leg_front_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, -1*stp.eefm_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*stp.eefm_leg_rear_margin, stp.eefm_leg_outside_margin])] + astp.eefm_leg_inside_margin=0.05 + astp.eefm_leg_outside_margin=0.05 + astp.eefm_leg_front_margin=0.16 + astp.eefm_leg_rear_margin=0.06 + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, -1*astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, -1*astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, astp.eefm_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, astp.eefm_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[astp.eefm_leg_front_margin, -1*astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, -1*astp.eefm_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*astp.eefm_leg_rear_margin, astp.eefm_leg_outside_margin])] rarm_vertices = rleg_vertices 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] - self.st_svc.setParameter(stp) + astp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices, rarm_vertices, larm_vertices]) + astp.eefm_cogvel_cutoff_freq = 4.0 + astp.eefm_k1=[-1.36334,-1.36334] + astp.eefm_k2=[-0.343983,-0.343983] + astp.eefm_k3=[-0.161465,-0.161465] + astp.swing2landing_transition_time = 0.05 + astp.landing_phase_time = 0.3 + astp.landing2support_transition_time = 0.1 + astp.surpport_phase_min_time = 0.3 + astp.support2swing_transition_time = 0.1 + leg_gains = {"support_pgain":[5,30,10,5,0.5,0.1], "support_dgain":[70,70,50,40,1,3], + "landing_pgain":[5,30,10,1,0.5,0.1], "landing_dgain":[70,70,50,10,1,3], + "swing_pgain":[5,30,10,5,0.5,0.1], "swing_dgain":[70,70,50,40,1,3]} # for walking + 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], + "swing_pgain":[100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100]} # normal arm gain + astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) + astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE + self.abc_svc.setStabilizerParam(astp) + # rh setting + if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: + 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] #gg.default_step_time=1.0 #self.abc_svc.setGaitGeneratorParam(gg) gg=self.abc_svc.getGaitGeneratorParam()[1] - gg.default_step_time=1.2 - gg.default_step_height=0.065 + gg.default_step_time=0.8 + gg.default_step_height=0.07 #gg.default_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 + gg.default_double_support_ratio=0.15 #gg.stride_parameter=[0.1,0.05,10.0] #gg.default_step_time=1.0 - #gg.swing_trajectory_delay_time_offset=0.35 - #gg.swing_trajectory_delay_time_offset=0.2 - gg.swing_trajectory_delay_time_offset=0.15 + gg.swing_trajectory_delay_time_offset=0.238 gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] gg.swing_trajectory_final_distance_weight=3.0 - gg.default_orbit_type = OpenHRP.AutoBalancerService.CYCLOIDDELAY + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE gg.toe_pos_offset_x = 1e-3*117.338; gg.heel_pos_offset_x = 1e-3*-116.342; gg.toe_zmp_offset_x = 1e-3*117.338; gg.heel_zmp_offset_x = 1e-3*-116.342; gg.optional_go_pos_finalize_footstep_num=1 - gg.front_edge_offset_of_steppable_region = [-0.02, 0.0] + gg.overwritable_footstep_index_offset=1 + gg.leg_margin = [0.16, 0.06, 0.05, 0.05] + gg.safe_leg_margin = [0.12, 0.02, 0.05, 0.05] + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.stride_limitation_for_circle_type = [0.15, 0.3, 15, 0.15, 0.135] + gg.overwritable_stride_limitation = [0.3, 0.45, 0, 0.3, 0.125] + gg.margin_time_ratio = 0.0 + gg.min_time_margin = 0.12 + gg.min_time = 0.7 + gg.overwritable_max_time = 1.0 + gg.modify_footsteps = True + gg.use_act_states = True + gg.emergency_step_time = [0.02, 0.6, 0.7] + gg.rectangle_goal_off = [0, 0, -0.05] + gg.dcm_offset = 0.01 self.abc_svc.setGaitGeneratorParam(gg) # Ic setting limbs = ['rarm', 'larm'] @@ -477,7 +510,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): self.ic_svc.setImpedanceControllerParam(l, icp) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] - esp.default_recover_time=10.0 # [s] + esp.default_recover_time=3.0 # [s] esp.default_retrieve_time=1.0 # [s] esp.default_retrieve_duration=1.0 # [s] self.es_svc.setEmergencyStopperParam(esp) From ad4472615a82e0d68c25b9f3cc1b6dd59f36edbd Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 15 Feb 2022 17:54:13 +0900 Subject: [PATCH 81/83] up --- .../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 e54a7ac4..c69601ff 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 @@ -459,7 +459,7 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): "landing_pgain":[100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100]} # normal arm gain astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) - astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE + # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) # rh setting if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: From ba5f3ec4cc8e5d8f4067a93ecf9ba29f3ca02c8c Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 23 Feb 2022 17:23:11 +0900 Subject: [PATCH 82/83] up --- .../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 c69601ff..adaf02ed 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 @@ -247,8 +247,8 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] @@ -416,8 +416,8 @@ def setStAbcIcParametersJAXON_BLUE(self, foot="KAWADA"): astp.eefm_swing_damping_moment_thre=[15]*3 astp.eefm_use_swing_damping=True # astp.eefm_ee_error_cutoff_freq=10000 # not used - astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_rot_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + # astp.eefm_swing_pos_spring_gain=[[5.0, 5.0, 5.0], [5.0, 5.0, 5.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] astp.eefm_swing_rot_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_swing_pos_time_const=[[1.0, 1.0, 1.0]]*4 astp.eefm_ee_moment_limit = [[90.0,90.0,1e4], [90.0,90.0,1e4], [1e4]*3, [1e4]*3] From be0518032b3e447753647d3bfd2c96d71b13c3a7 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Thu, 31 Mar 2022 16:38:57 +0900 Subject: [PATCH 83/83] use torque mode --- .../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 adaf02ed..fe8a3a5c 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 @@ -318,7 +318,7 @@ def setStAbcIcParametersJAXON(self, foot="KAWADA"): "landing_pgain":[100,100,100,100,100,100,100,100], "landing_dgain":[100,100,100,100,100,100,100,100], "swing_pgain":[100,100,100,100,100,100,100,100], "swing_dgain":[100,100,100,100,100,100,100,100]} astp.joint_servo_control_parameters = map (lambda x : OpenHRP.AutoBalancerService.JointServoControlParameter(**x), [leg_gains,leg_gains,arm_gains,arm_gains]) - # astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE + astp.joint_control_mode = OpenHRP.RobotHardwareService.TORQUE self.abc_svc.setStabilizerParam(astp) # rh setting if astp.joint_control_mode == OpenHRP.RobotHardwareService.TORQUE: