From 8f43b126bf63782e6d4c3c31f064e106a59da588 Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Sat, 22 Dec 2018 19:22:48 +0900 Subject: [PATCH 1/3] Separate urata_hrpsys_config.py to each robot config --- .../chidori_hrpsys_config.py | 108 ++- .../jaxon_blue_hrpsys_config.py | 139 ++- .../jaxon_hrpsys_config.py | 154 +++- .../jaxon_red_hrpsys_config.py | 63 +- .../staro_hrpsys_config.py | 120 ++- .../tqleg0_hrpsys_config.py | 72 +- .../urata_hrpsys_config.py | 861 +++--------------- .../urataleg_hrpsys_config.py | 66 +- .../ystleg_hrpsys_config.py | 11 +- 9 files changed, 847 insertions(+), 747 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py index 6d8d908e..56617af3 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py @@ -2,8 +2,114 @@ from urata_hrpsys_config import * +class CHIDORIHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "CHIDORI") + + def resetPose(self): + return [0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, # rleg + 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0] # lleg + + def resetLandingPose (self): + # handmade + return [0.0, 0.0, -0.698132, 1.39626, -0.698132, 0.0, # rleg + 0.0, 0.0, -0.698132, 1.39626, -0.698132, 0.0] # lleg + + def defJointGroups(self): + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + self.Groups = [rleg_group, lleg_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + #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 + self.setABCParameters(abcp) + + def setDefaultGaitGeneraterParameters(self): + gg = self.getGaitGeneraterParameters() + gg.default_step_time=1.2 + # gg.default_double_support_ratio=0.32 + gg.default_double_support_ratio=0.35 + # gg.stride_parameter=[0.1,0.05,10.0] + # gg.default_step_time=1.0 + 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.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; + self.setGaitGeneraterParameters(gg) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOP + 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*0.5, 20*1.6*1.1*1.5*0.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 + # 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.05 + tmp_leg_outside_margin=0.05 + tmp_leg_front_margin=0.12 + 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 + # 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.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])] + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + 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.setSTParameters(stp) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("CHIDORI") + hcf = CHIDORIHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py index 3569cd14..1e9b8cd8 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py @@ -2,8 +2,145 @@ from urata_hrpsys_config import * +class JAXON_BLUEHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "JAXON_BLUE") + + def resetPose(self): + ## Different from (send *robot* :reset-pose) + return [0,0,-0.244732,0.676564,-0.431836,0, 0,0,-0.244735,0.676565,-0.431834,0, 0,0,0, 0,0, 0.698132,-0.349066,-0.087266,-1.39626,0,0,-0.349066, 0.698132,0.349066,0.087266,-1.39626,0,0,-0.349066] + + def resetManipPose (self): + ## Different from (send *robot* :reset-manip-pose) + return [0,0,-0.237906,0.673927,-0.436025,-0.000427, 0,0,-0.237909,0.673928,-0.436023,-0.000428, 0,0,0, 0,0.523599, 0.959931,-0.349066,-0.261799,-1.74533,-0.436332,0,-0.785398, 0.959931,0.349066,0.261799,-1.74533,0.436332,0,-0.785398] + + def resetLandingPose (self): + return self.resetPose() + + def collisionFreeInitPose (self): + return [0.0,0.0,0.0,0.0,0.0,0.0, 0.0,0.0,0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0, 0.0,-0.261799,0.0,0.0,0.0,0.0,0.0, 0.0,0.261799,0.0,0.0,0.0,0.0,0.0] + + def collisionFreeResetPose (self): + return [0.0,0.0,-0.349066,0.698132,-0.349066,0.0, 0.0,0.0,-0.349066,0.698132,-0.349066,0.0, 0.0,0.0,0.0, 0.0,0.0, 0.0,-0.523599,0.0,0.0,0.0,0.0,0.0, 0.0,0.523599,0.0,0.0,0.0,0.0,0.0] + + + def defJointGroups(self): + rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6']] + larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6']] + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] + torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] + self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultESParameters(self): + esp = self.getESParameters() + esp.default_recover_time=10.0 # [s] + esp.default_retrieve_time=1.0 # [s] + self.setESParameters(esp) + + def setDefaultICParameters(self): + limbs = ['rarm', 'larm'] + for l in limbs: + icp = self.getICParameters(l) + icp.D_p = 600 + icp.D_r = 200 + self.setICParameters(l, icp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + abcp.default_zmp_offsets=[[0.05, 0.0, 0.0], [0.05, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultGaitGeneraterParameters(self): + gg = self.getGaitGeneraterParameters() + gg.default_step_time=1.2 + gg.default_step_height=0.065 + #gg.default_double_support_ratio=0.32 + gg.default_double_support_ratio=0.35 + #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.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.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 + self.setGaitGeneraterParameters(gg) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + #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 + [25, 25, 1e5], + [63.36, 63.36, 1e5], + [63.36, 63.36, 1e5]] + stp.eefm_pos_damping_gain = [[33600.0, 33600.0, 3234.0], # modification with kojio xy=10000? + [33600.0, 33600.0, 3234.0], + [26880.0, 26880.0, 7392.0], + [26880.0, 26880.0, 7392.0]] + stp.eefm_swing_pos_damping_gain = stp.eefm_pos_damping_gain[0] # same with support leg + stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] # same with support leg + stp.eefm_rot_compensation_limit = [math.radians(10), math.radians(10), math.radians(10), math.radians(10)] + stp.eefm_pos_compensation_limit = [0.025, 0.025, 0.050, 0.050] + stp.eefm_swing_damping_force_thre=[200]*3 + stp.eefm_swing_damping_moment_thre=[15]*3 + stp.eefm_use_swing_damping=True + stp.eefm_ee_error_cutoff_freq=20.0 + # stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*4 + # stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*4 + stp.eefm_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 + 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])] + 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.setSTParameters(stp) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("JAXON_BLUE") + hcf = JAXON_BLUEHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py index 383a5f59..09c378d9 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py @@ -2,8 +2,160 @@ from urata_hrpsys_config import * +class JAXONHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "JAXON") + + def resetPose (self): + ## Different from (send *robot* :reset-pose) + return [-8.002327e-06,0.000427,-0.244732,0.676564,-0.431836,-0.000427,-8.669072e-06,0.000428,-0.244735,0.676565,-0.431834,-0.000428,0.0,0.0,0.0,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] + + def resetManipPose(self): + ## Different from (send *robot* :reset-manip-pose) + return [-7.516871e-06,0.000427,-0.237906,0.673927,-0.436025,-0.000427,-8.257966e-06,0.000428,-0.237909,0.673928,-0.436023,-0.000428,0.0,0.0,0.0,0.0,0.523599,0.0,0.959931,-0.349066,-0.261799,-1.74533,-0.436332,0.0,-0.785398,0.0,0.959931,0.349066,0.261799,-1.74533,0.436332,0.0,-0.785398] + + def resetLandingPose(self): + # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose))) + return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] + + def collisionFreeInitPose(self): + return [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.261799,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.261799,0.0,0.0,0.0,0.0,0.0] + + def collisionFreeResetPose(self): + return [0.0,0.0,-0.349066,0.698132,-0.349066,0.0,0.0,0.0,-0.349066,0.698132,-0.349066,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.523599,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.523599,0.0,0.0,0.0,0.0,0.0] + + def defJointGroups(self): + rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']] + larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']] + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] + torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] + self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultForceMomentOffset(self): + import rospkg + self.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_THK003_CLOSE_JAXON") + + def setDefaultESParameters(self): + esp = self.getESParameters() + esp.default_recover_time=10.0 # [s] + esp.default_retrieve_time=1.0 # [s] + self.setESParameters(esp) + + def setDefaultICParameters(self): + limbs = ['rarm', 'larm'] + for l in limbs: + icp = self.getICParameters(l) + icp.D_p = 600 + icp.D_r = 200 + self.setICParameters(l, icp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + #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]] + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultGaitGeneraterParameters(self): + gg = self.getGaitGeneraterParameters() + gg.default_step_time=1.2 + gg.default_step_height=0.065 + #gg.default_double_support_ratio=0.32 + gg.default_double_support_ratio=0.35 + #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.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.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 + self.setGaitGeneraterParameters(gg) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + #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] + 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] + 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 + 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.setSTParameters(stp) + self.setJAXONFootMarginParam(foot="LEPTRINO") + + def setJAXONFootMarginParam(self, foot="KAWADA"): + stp=self.getSTParameters() + 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]) + self.setSTParameters(stp) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("JAXON") + hcf = JAXONHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py index eab1c30a..d29cf889 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py @@ -1,9 +1,68 @@ #!/usr/bin/env python -from urata_hrpsys_config import * +from jaxon_hrpsys_config import * + +class JAXON_REDHrpsysConfigurator(JAXONHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "JAXON_RED") + + def setDefaultForceMomentOffset(self): + import rospkg + self.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_JAXON_RED") + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + #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.01, 0.0], [0.0, -0.01, 0.0], [0, 0, 0], [0, 0, 0]] + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultSTParameters(self): + stp=self.getSTParameters() + #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] + 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 + 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.setSTParameters(stp) + self.setJAXONFootMarginParam(foot="KAWADA") if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("JAXON_RED") + hcf = JAXON_REDHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py index 875fb133..d9d5dba1 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py @@ -2,8 +2,126 @@ from urata_hrpsys_config import * +class STAROHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "STARO") + + def resetPose(self): + return [0.174533, 2.0944, 1.5708, 1.5708, -1.0472, 0.0, 0.0, 0.785398, -0.174533, -1.0472, 1.5708, -1.5708, 1.0472, 0.0, 0.0, 0.785398, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, 0.0, 0.0] + + def resetManipPose(self): + return [0.174533, 2.61799, 1.5708, 1.5708, -1.91986, 0.0, -0.698132, 0.785398, -0.174533, -0.523599, 1.5708, -1.5708, 1.91986, 0.0, 0.698132, 0.785398, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, 0.0, 0.0] + + def resetServoOffPose(self): + return [1.39626, 1.5708, 0.698132, 1.74533, -0.261799, 0.0, 0.0, -0.785398, -1.39626, -1.5708, 0.698132, -1.39626, 0.261799, 0.0, 0.0, -0.785398, 0.0, 0.0, -0.174533, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.174533, 0.698132, -0.349066, 0.0, 0.0, -0.087266, 0.872665, 0.0] + + def defJointGroups(self): + # Is it correct? + rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']] + larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']] + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] + torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] + self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + abcp.default_zmp_offsets=[[0.00, 0.0, 0.0], [0.00, 0.0, 0.0], [0, 0, 0], [0, 0, 0]] + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultGaitGeneraterParameters(self): + gg = self.getGaitGeneraterParameters() + gg.default_step_time=1.2 + #gg.default_double_support_ratio=0.32 + gg.default_double_support_ratio=0.35 + #gg.stride_parameter=[0.1,0.05,10.0] + #gg.default_step_time=1.0 + gg.swing_trajectory_delay_time_offset=0.35 + gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] + self.setGaitGeneraterParameters(gg) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + stp.st_algorithm=OpenHRP.StabilizerService.EEFM + #stp.emergency_check_mode=OpenHRP.StabilizerService.CP + #stp.cp_check_margin=80*1e-3 + 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=120 + #stp.eefm_rot_damping_gain=70 + #stp.eefm_rot_damping_gain=60 + # old + #stp.eefm_rot_damping_gain=20*1.6 + # new, servo-off-walking-pose + #stp.eefm_rot_damping_gain=20*1.75 + # new, reset-manip-psoe + stp.eefm_rot_damping_gain = [[20*2.0, 20*2.0, 1e5]]*4 + #stp.eefm_rot_damping_gain=100 + #stp.eefm_pos_damping_gain=3500*2.5 + #stp.eefm_pos_damping_gain=7800 + stp.eefm_pos_damping_gain = [[3500*1.6*3, 3500*1.6*3*0.07, 3500*1.6]]*4 + #stp.eefm_rot_time_const=1.0 + #stp.eefm_pos_time_const_support=1.0 + stp.eefm_rot_time_const = [[1.5, 1.5, 1.5]]*4 + stp.eefm_pos_time_const_support = [[1.5, 1.5, 1.5]]*4 + stp.eefm_wrench_alpha_blending=0.7 + #stp.eefm_pos_time_const_swing=0.03 + 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 + # 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_inside_margin=0.04 + stp.eefm_leg_front_margin=0.12 + stp.eefm_leg_rear_margin=0.09 + 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])] + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_zmp_delay_time_const=[0.055, 0.055] + #stp.eefm_cogvel_cutoff_freq = 3.18 + #stp.eefm_cogvel_cutoff_freq = 6.18 # servooff+walk + #stp.eefm_cogvel_cutoff_freq = 5.5 + #stp.eefm_cogvel_cutoff_freq = 4.5 + #stp.eefm_cogvel_cutoff_freq = 3.5 # reset-pose + #stp.eefm_cogvel_cutoff_freq = 5.0 + # old + #stp.eefm_cogvel_cutoff_freq = 4.25 + # new + stp.eefm_cogvel_cutoff_freq = 4.0 + # old + # stp.eefm_k1=[-1.46719, -1.46719] + # stp.eefm_k2=[-0.466538, -0.466538] + # stp.eefm_k3=[-0.194206, -0.194206] + # new, servo-off-walking-pose + # stp.eefm_k1=[-1.45044,-1.45044] + # stp.eefm_k2=[-0.44668,-0.44668] + # stp.eefm_k3=[-0.190114,-0.190114] + # new, reset-manip-pose + stp.eefm_k1=[-1.48696,-1.48696] + stp.eefm_k2=[-0.490137,-0.490137] + stp.eefm_k3=[-0.198649,-0.198649] + # new, reset-manip-pose + self.setSTParameters(stp) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("STARO") + hcf = STAROHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py index 1589f9d6..f111150a 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py @@ -2,8 +2,78 @@ from urata_hrpsys_config import * +class TQLEG0HrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "TQLEG0") + + def resetPose(self): + return [0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0, 0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0] + + def defJointGroups(self): + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + self.Groups = [rleg_group, lleg_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + #abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; + abcp.default_zmp_offsets=[[0.0, 0.045, 0.0], [0.0, -0.045, 0.0]]; + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + stp.st_algorithm=OpenHRP.StabilizerService.EEFM + #####stp.is_ankle_torque_enable=[True,False] + 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.4*5, 20*1.4*5, 1e5]]*2 + stp.eefm_pos_damping_gain=[[3500*10, 3500*10, 3500*1.1*1.5]]*2 + stp.eefm_rot_time_const=[[1.5, 1.5, 1.5]]*2 + stp.eefm_pos_time_const_support=[[1.5, 1.5, 1.5]]*2 + stp.eefm_wrench_alpha_blending=0.8 + 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 + # mechanical param is => inside 0.055, front 0.13, rear 0.1 + ## kawada ashi + # stp.eefm_leg_inside_margin=0.05 + # stp.eefm_leg_front_margin=0.12 + # stp.eefm_leg_rear_margin=0.09 + ## leptrino ashi + stp.eefm_leg_inside_margin=0.135*0.5 + stp.eefm_leg_front_margin=0.16 + stp.eefm_leg_rear_margin=0.1 + 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])] + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_cogvel_cutoff_freq = 4.25 + stp.eefm_k1=[-1.38077, -1.38077] + stp.eefm_k2=[-0.364652, -0.364652] + stp.eefm_k3=[-0.168538, -0.168538] + self.setSTParameters(stp) + + def setDefaultServoErrorLimit(self): + # remove soft-error-limit for torque controlled robot ( ONLY FOR THIS ROBOT !!! ) + self.el_svc.setServoErrorLimit("ALL", 100000) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("TQLEG0") + hcf = TQLEG0HrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : 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..15c9e44a 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,742 +19,127 @@ 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: - self.setStAbcParameters() + self.setDefaultHrpsysParameters() + + # Wrapper functions to get hrpsys parameters + def getKFParameters(self): + return self.kf_svc.getKalmanFilterParam()[1] + + def getESParameters(self): + return self.es_svc.getEmergencyStopperParam()[1] + + def getICParameters(self, limb): + return self.ic_svc.getImpedanceControllerParam(limb)[1] + + def getABCParameters(self): + return self.abc_svc.getAutoBalancerParam()[1] + + def getGaitGeneraterParameters(self): + return self.abc_svc.getGaitGeneratorParam()[1] + + def getSTParameters(self): + return self.st_svc.getParameter() + + # Wrapper functions to set hrpsys parameters + def setKFParameters(self, param): + self.kf_svc.setKalmanFilterParam(param) + + def loadForceMomentOffsetParams(self, file_path): + self.rmfo_svc.loadForceMomentOffsetParams(file_path) + + def setESParameters(self, param): + self.es_svc.setEmergencyStopperParam(param) + + def setICParameters(self, limb, param): + self.ic_svc.setImpedanceControllerParam(limb, param) + + def setABCParameters(self, param): + self.abc_svc.setAutoBalancerParam(param) + + def setGaitGeneraterParameters(self, param): + self.abc_svc.setGaitGeneratorParam(param) + + def setSTParameters(self, param): + self.st_svc.setParameter(param) + + def setServoErrorLimit(self, joint_name, limit): + self.el_svc.setServoErrorLimit(joint_name, limit) + + # Functions to set default hrpsys parameters for each robot + def setDefaultHrpsysParameters(self): + if self.kf: + self.setDefaultKFParameters() if self.rmfo: - self.loadForceMomentOffsetFile() - - def defJointGroups (self): - if self.ROBOT_NAME == "URATALEG": - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] - self.Groups = [rleg_group, lleg_group] - elif self.ROBOT_NAME == "YSTLEG": - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT4_2', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT4_2', 'LLEG_JOINT5']] - self.Groups = [rleg_group, lleg_group] - elif self.ROBOT_NAME == "CHIDORI": - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] - self.Groups = [rleg_group, lleg_group] - elif self.ROBOT_NAME == "TQLEG0": - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] - self.Groups = [rleg_group, lleg_group] - elif self.ROBOT_NAME == "JAXON_BLUE": - rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6']] - larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6']] - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] - head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] - torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] - self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] - else: - rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']] - larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']] - rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] - lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] - head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] - torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']] - self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group] - - 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="KAWADA") - elif self.ROBOT_NAME == "JAXON_BLUE": - self.setStAbcIcParametersJAXON_BLUE() - elif self.ROBOT_NAME == "URATALEG": - self.setStAbcParametersURATALEG() - elif self.ROBOT_NAME == "YSTLEG": - self.setStAbcParametersYSTLEG() - elif self.ROBOT_NAME == "CHIDORI": - self.setStAbcParametersCHIDORI() - elif self.ROBOT_NAME == "TQLEG0": - self.setStAbcParametersTQLEG0() - - def setStAbcParametersSTARO (self): - # abc setting - abcp=self.abc_svc.getAutoBalancerParam()[1] - abcp.default_zmp_offsets=[[0.00, 0.0, 0.0], [0.00, 0.0, 0.0], [0, 0, 0], [0, 0, 0]]; - abcp.move_base_gain=0.8 - 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.emergency_check_mode=OpenHRP.StabilizerService.CP - #stp.cp_check_margin=80*1e-3 - 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=120 - #stp.eefm_rot_damping_gain=70 - #stp.eefm_rot_damping_gain=60 - # old - #stp.eefm_rot_damping_gain=20*1.6 - # new, servo-off-walking-pose - #stp.eefm_rot_damping_gain=20*1.75 - # new, reset-manip-psoe - stp.eefm_rot_damping_gain = [[20*2.0, 20*2.0, 1e5]]*4 - #stp.eefm_rot_damping_gain=100 - #stp.eefm_pos_damping_gain=3500*2.5 - #stp.eefm_pos_damping_gain=7800 - stp.eefm_pos_damping_gain = [[3500*1.6*3, 3500*1.6*3*0.07, 3500*1.6]]*4 - #stp.eefm_rot_time_const=1.0 - #stp.eefm_pos_time_const_support=1.0 - stp.eefm_rot_time_const = [[1.5, 1.5, 1.5]]*4 - stp.eefm_pos_time_const_support = [[1.5, 1.5, 1.5]]*4 - stp.eefm_wrench_alpha_blending=0.7 - #stp.eefm_pos_time_const_swing=0.03 - 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 - # 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_inside_margin=0.04 - stp.eefm_leg_front_margin=0.12 - stp.eefm_leg_rear_margin=0.09 - 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])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) - stp.eefm_zmp_delay_time_const=[0.055, 0.055] - #stp.eefm_cogvel_cutoff_freq = 3.18 - #stp.eefm_cogvel_cutoff_freq = 6.18 # servooff+walk - #stp.eefm_cogvel_cutoff_freq = 5.5 - #stp.eefm_cogvel_cutoff_freq = 4.5 - #stp.eefm_cogvel_cutoff_freq = 3.5 # reset-pose - #stp.eefm_cogvel_cutoff_freq = 5.0 - # old - #stp.eefm_cogvel_cutoff_freq = 4.25 - # new - stp.eefm_cogvel_cutoff_freq = 4.0 - # old - # stp.eefm_k1=[-1.46719, -1.46719] - # stp.eefm_k2=[-0.466538, -0.466538] - # stp.eefm_k3=[-0.194206, -0.194206] - # new, servo-off-walking-pose - # stp.eefm_k1=[-1.45044,-1.45044] - # stp.eefm_k2=[-0.44668,-0.44668] - # stp.eefm_k3=[-0.190114,-0.190114] - # new, reset-manip-pose - stp.eefm_k1=[-1.48696,-1.48696] - stp.eefm_k2=[-0.490137,-0.490137] - stp.eefm_k3=[-0.198649,-0.198649] - # new, reset-manip-pose - self.st_svc.setParameter(stp) - # 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_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 - #gg.stride_parameter=[0.1,0.05,10.0] - #gg.default_step_time=1.0 - gg.swing_trajectory_delay_time_offset=0.35 - gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] - self.abc_svc.setGaitGeneratorParam(gg) - - - def setStAbcIcParametersJAXON(self, foot="KAWADA"): - # abc setting - abcp=self.abc_svc.getAutoBalancerParam()[1] - #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]]; - 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 - 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 # 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) - # 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_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 - #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.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.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 - self.abc_svc.setGaitGeneratorParam(gg) - # Ic setting - limbs = ['rarm', 'larm'] - for l in limbs: - icp = self.ic_svc.getImpedanceControllerParam(l)[1] - icp.D_p = 600 - icp.D_r = 200 - self.ic_svc.setImpedanceControllerParam(l, icp) - # Estop - esp=self.es_svc.getEmergencyStopperParam()[1] - esp.default_recover_time=10.0 # [s] - esp.default_retrieve_time=1.0 # [s] - self.es_svc.setEmergencyStopperParam(esp) - - 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.move_base_gain=0.8 - 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 - [25, 25, 1e5], - [63.36, 63.36, 1e5], - [63.36, 63.36, 1e5]] - stp.eefm_pos_damping_gain = [[33600.0, 33600.0, 3234.0], # modification with kojio xy=10000? - [33600.0, 33600.0, 3234.0], - [26880.0, 26880.0, 7392.0], - [26880.0, 26880.0, 7392.0]] - stp.eefm_swing_pos_damping_gain = stp.eefm_pos_damping_gain[0] # same with support leg - stp.eefm_swing_rot_damping_gain = stp.eefm_rot_damping_gain[0] # same with support leg - stp.eefm_rot_compensation_limit = [math.radians(10), math.radians(10), math.radians(10), math.radians(10)] - stp.eefm_pos_compensation_limit = [0.025, 0.025, 0.050, 0.050] - stp.eefm_swing_damping_force_thre=[200]*3 - stp.eefm_swing_damping_moment_thre=[15]*3 - stp.eefm_use_swing_damping=True - stp.eefm_ee_error_cutoff_freq=20.0 - # stp.eefm_swing_rot_spring_gain=[[1.0, 1.0, 1.0]]*4 - # stp.eefm_swing_pos_spring_gain=[[1.0, 1.0, 1.0]]*4 - stp.eefm_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 - 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])] - 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) - # 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_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 - #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.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.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 - self.abc_svc.setGaitGeneratorParam(gg) - # Ic setting - limbs = ['rarm', 'larm'] - for l in limbs: - icp = self.ic_svc.getImpedanceControllerParam(l)[1] - icp.D_p = 600 - icp.D_r = 200 - self.ic_svc.setImpedanceControllerParam(l, icp) - # Estop - esp=self.es_svc.getEmergencyStopperParam()[1] - esp.default_recover_time=10.0 # [s] - esp.default_retrieve_time=1.0 # [s] - self.es_svc.setEmergencyStopperParam(esp) - - def setStAbcParametersURATALEG (self): - # abc setting - abcp=self.abc_svc.getAutoBalancerParam()[1] - abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; - abcp.move_base_gain=0.8 - 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.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.4*5, 20*1.4*5, 1e5]]*2 - stp.eefm_pos_damping_gain=[[3500*10, 3500*10, 3500*1.1*5]]*2 - stp.eefm_rot_time_const=[[1.5, 1.5, 1.5]]*2 - stp.eefm_pos_time_const_support=[[1.5, 1.5, 1.5]]*2 - stp.eefm_wrench_alpha_blending=0.8 - 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 - # mechanical param is => inside 0.055, front 0.13, rear 0.1 - ## kawada ashi - # stp.eefm_leg_inside_margin=0.05 - # stp.eefm_leg_front_margin=0.12 - # stp.eefm_leg_rear_margin=0.09 - ## leptrino ashi - stp.eefm_leg_inside_margin=0.135*0.5 - stp.eefm_leg_front_margin=0.16 - stp.eefm_leg_rear_margin=0.1 - 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])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) - stp.eefm_zmp_delay_time_const=[0.055, 0.055] - stp.eefm_cogvel_cutoff_freq = 4.25 - stp.eefm_k1=[-1.38077, -1.38077] - stp.eefm_k2=[-0.364652, -0.364652] - stp.eefm_k3=[-0.168538, -0.168538] - self.st_svc.setParameter(stp) - - def setStAbcParametersYSTLEG (self): - print "Not implemented yet" - - 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.move_base_gain=0.8 - 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.EEFMQPCOP - 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*0.5, 20*1.6*1.1*1.5*0.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 - # 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.05 - tmp_leg_outside_margin=0.05 - tmp_leg_front_margin=0.12 - 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 - # 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.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])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.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.st_svc.setParameter(stp) - # 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_double_support_ratio=0.32 - gg.default_double_support_ratio=0.35 - #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.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.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; - self.abc_svc.setGaitGeneratorParam(gg) - - def setStAbcParametersTQLEG0 (self): - # abc setting - abcp=self.abc_svc.getAutoBalancerParam()[1] - #abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; - abcp.default_zmp_offsets=[[0.0, 0.045, 0.0], [0.0, -0.045, 0.0]]; - abcp.move_base_gain=0.8 - 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.is_ankle_torque_enable=[True,False] - 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.4*5, 20*1.4*5, 1e5]]*2 - stp.eefm_pos_damping_gain=[[3500*10, 3500*10, 3500*1.1*1.5]]*2 - stp.eefm_rot_time_const=[[1.5, 1.5, 1.5]]*2 - stp.eefm_pos_time_const_support=[[1.5, 1.5, 1.5]]*2 - stp.eefm_wrench_alpha_blending=0.8 - 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 - # mechanical param is => inside 0.055, front 0.13, rear 0.1 - ## kawada ashi - # stp.eefm_leg_inside_margin=0.05 - # stp.eefm_leg_front_margin=0.12 - # stp.eefm_leg_rear_margin=0.09 - ## leptrino ashi - stp.eefm_leg_inside_margin=0.135*0.5 - stp.eefm_leg_front_margin=0.16 - stp.eefm_leg_rear_margin=0.1 - 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])] - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) - stp.eefm_zmp_delay_time_const=[0.055, 0.055] - stp.eefm_cogvel_cutoff_freq = 4.25 - stp.eefm_k1=[-1.38077, -1.38077] - stp.eefm_k2=[-0.364652, -0.364652] - stp.eefm_k3=[-0.168538, -0.168538] - self.st_svc.setParameter(stp) - # el setting - # remove soft-error-limit for torque controlled robot ( ONLY FOR THIS ROBOT !!! ) - self.el_svc.setServoErrorLimit("ALL", 100000) - - - def jaxonResetPose (self): - ## Different from (send *robot* :reset-pose) - return [-8.002327e-06,0.000427,-0.244732,0.676564,-0.431836,-0.000427,-8.669072e-06,0.000428,-0.244735,0.676565,-0.431834,-0.000428,0.0,0.0,0.0,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] - def jaxonBlueResetPose (self): - ## Different from (send *robot* :reset-pose) - return [0,0,-0.244732,0.676564,-0.431836,0, 0,0,-0.244735,0.676565,-0.431834,0, 0,0,0, 0,0, 0.698132,-0.349066,-0.087266,-1.39626,0,0,-0.349066, 0.698132,0.349066,0.087266,-1.39626,0,0,-0.349066] - - def staroResetPose(self): - return [0.174533, 2.0944, 1.5708, 1.5708, -1.0472, 0.0, 0.0, 0.785398, -0.174533, -1.0472, 1.5708, -1.5708, 1.0472, 0.0, 0.0, 0.785398, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, 0.0, 0.0] - - def uratalegResetPose(self): - return [0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0, 0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0] - - def jaxonResetManipPose (self): - ## Different from (send *robot* :reset-manip-pose) - return [-7.516871e-06,0.000427,-0.237906,0.673927,-0.436025,-0.000427,-8.257966e-06,0.000428,-0.237909,0.673928,-0.436023,-0.000428,0.0,0.0,0.0,0.0,0.523599,0.0,0.959931,-0.349066,-0.261799,-1.74533,-0.436332,0.0,-0.785398,0.0,0.959931,0.349066,0.261799,-1.74533,0.436332,0.0,-0.785398] - - def jaxonBlueResetManipPose (self): - ## Different from (send *robot* :reset-manip-pose) - return [0,0,-0.237906,0.673927,-0.436025,-0.000427, 0,0,-0.237909,0.673928,-0.436023,-0.000428, 0,0,0, 0,0.523599, 0.959931,-0.349066,-0.261799,-1.74533,-0.436332,0,-0.785398, 0.959931,0.349066,0.261799,-1.74533,0.436332,0,-0.785398] - - def staroResetManipPose(self): - return [0.174533, 2.61799, 1.5708, 1.5708, -1.91986, 0.0, -0.698132, 0.785398, -0.174533, -0.523599, 1.5708, -1.5708, 1.91986, 0.0, 0.698132, 0.785398, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.349066, 0.698132, -0.349066, 0.0, 0.0, 0.0, 0.0, 0.0] - - def jaxonInitPose (self): - return [0]*len(self.jaxonResetPose()) - - def jaxonBlueInitPose (self): - return [0]*len(self.jaxonBlueResetPose()) - - def staroInitPose (self): - return [0]*len(self.staroResetPose()) - - def uratalegInitPose (self): - return [0]*len(self.uratalegResetPose()) - - def jaxonCollisionFreeInitPose (self): - return [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.261799,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.261799,0.0,0.0,0.0,0.0,0.0] - - def jaxonBlueCollisionFreeInitPose (self): - return [0.0,0.0,0.0,0.0,0.0,0.0, 0.0,0.0,0.0,0.0,0.0,0.0, 0.0,0.0,0.0, 0.0,0.0, 0.0,-0.261799,0.0,0.0,0.0,0.0,0.0, 0.0,0.261799,0.0,0.0,0.0,0.0,0.0] - - def jaxonCollisionFreeResetPose (self): - return [0.0,0.0,-0.349066,0.698132,-0.349066,0.0,0.0,0.0,-0.349066,0.698132,-0.349066,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.523599,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.523599,0.0,0.0,0.0,0.0,0.0] - - def jaxonBlueCollisionFreeResetPose (self): - return [0.0,0.0,-0.349066,0.698132,-0.349066,0.0, 0.0,0.0,-0.349066,0.698132,-0.349066,0.0, 0.0,0.0,0.0, 0.0,0.0, 0.0,-0.523599,0.0,0.0,0.0,0.0,0.0, 0.0,0.523599,0.0,0.0,0.0,0.0,0.0] - - def staroResetServoOffPose(self): - return [1.39626, 1.5708, 0.698132, 1.74533, -0.261799, 0.0, 0.0, -0.785398, -1.39626, -1.5708, 0.698132, -1.39626, 0.261799, 0.0, 0.0, -0.785398, 0.0, 0.0, -0.174533, 0.698132, -0.349066, 0.0, 0.0, 0.0, -0.174533, 0.698132, -0.349066, 0.0, 0.0, -0.087266, 0.872665, 0.0] - - def chidoriResetPose (self): - return [0.0,0.0,-0.349066,0.698132,-0.349066,0.0,0.0,0.0,-0.349066,0.698132,-0.349066,0.0] - - def chidoriInitPose (self): - return [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] - - def tqleg0ResetPose(self): - return [0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0, 0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0] - - def tqleg0InitPose (self): - return [0]*len(self.tqleg0ResetPose()) - - # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose))) - def jaxonResetLandingPose (self): - return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066] - - def jaxonBlueResetLandingPose (self): - return self.jaxonBlueResetPose() - - # handmade - def chidoriResetLandingPose (self): - return [0.0,0.0,-0.698132,1.39626,-0.698132,0.0,0.0,0.0,-0.698132,1.39626,-0.698132,0.0] - - def setResetPose(self): - if self.ROBOT_NAME == "STARO": - self.seq_svc.setJointAngles(self.staroResetPose(), 5.0) - elif self.ROBOT_NAME == "JAXON_BLUE": - self.seq_svc.setJointAngles(self.jaxonBlueResetPose(), 5.0) - elif self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonResetPose(), 5.0) - elif self.ROBOT_NAME == "URATALEG": - self.seq_svc.setJointAngles(self.uratalegResetPose(), 5.0) - elif self.ROBOT_NAME == "CHIDORI": - self.seq_svc.setJointAngles(self.chidoriResetPose(), 5.0) - elif self.ROBOT_NAME == "TQLEG0": - self.seq_svc.setJointAngles(self.tqleg0ResetPose(), 5.0) - - def setResetManipPose(self): - if self.ROBOT_NAME == "STARO": - self.seq_svc.setJointAngles(self.staroResetManipPose(), 10.0) - elif self.ROBOT_NAME == "JAXON_BLUE": - self.seq_svc.setJointAngles(slef.jaxonBlueResetManipPose(), 10.0) - elif self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonResetManipPose(), 10.0) - - def setInitPose(self): - if self.ROBOT_NAME == "STARO": - self.seq_svc.setJointAngles(self.staroInitPose(), 10.0) - elif self.ROBOT_NAME == "JAXON_BLUE": - self.seq_svc.setJointAngles(self.jaxonBlueInitPose(), 10.0) - elif self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonInitPose(), 10.0) - elif self.ROBOT_NAME == "URATALEG": - self.seq_svc.setJointAngles(self.uratalegInitPose(), 10.0) - elif self.ROBOT_NAME == "CHIDORI": - self.seq_svc.setJointAngles(self.chidoriInitpose(), 10.0) - elif self.ROBOT_NAME == "TQLEG0": - self.seq_svc.setJointAngles(self.tqleg0Initpose(), 10.0) - - def setCollisionFreeInitPose(self): - if self.ROBOT_NAME == "JAXON_BLUE": - self.seq_svc.setJointAngles(self.jaxonBlueCollisionFreeInitPose(), 10.0) - elif self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonCollisionFreeInitPose(), 10.0) - - def setCollisionFreeResetPose(self): - if self.ROBOT_NAME == "STARO": - self.seq_svc.setJointAngles(self.staroResetServoOffPose(), 10.0) - elif self.ROBOT_NAME == "JAXON_BLUE": - self.seq_svc.setJointAngles(self.jaxonBlueCollisionFreeResetPose(), 10.0) - elif self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonCollisionFreeResetPose(), 10.0) - - def setResetLandingPose(self): - if self.ROBOT_NAME == 0: - self.seq_svc.setJointAngles(self.jaxonBlueResetLandingPose(), 5.0) - if self.ROBOT_NAME.find("JAXON") == 0: - self.seq_svc.setJointAngles(self.jaxonResetLandingPose(), 5.0) - if self.ROBOT_NAME.find("CHIDORI") == 0: - self.seq_svc.setJointAngles(self.chidoriResetLandingPose(), 5.0) - - def loadForceMomentOffsetFile (self): - import rospkg - if self.ROBOT_NAME == "JAXON": - self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_THK003_CLOSE_JAXON") - elif self.ROBOT_NAME == "JAXON_RED": - self.rmfo_svc.loadForceMomentOffsetParams(rospkg.RosPack().get_path('hrpsys_ros_bridge_tutorials')+"/models/hand_force_calib_offset_JAXON_RED") - else: - print "No force moment offset file" + self.setDefaultForceMomentOffset() + if self.es: + self.setDefaultESParameters() + if self.ic: + self.setDefaultICParameters() + if self.abc: + self.setDefaultABCParameters() + self.setDefaultGaitGeneraterParameters() + if self.st: + self.setDefaultSTParameters() + if self.el: + self.setDefaultServoErrorLimit() + + def setDefaultKFParameters(self): + print("setDefaultKFParameters is not implemented") + + def setDefaultForceMomentOffset(self): + print("No force moment offset file") + + def setDefaultESParameters(self): + print("setDefaultESParameters is not implemented") + + def setDefaultICParameters(self): + print("setDefaultICParameters is not implemented") + + def setDefaultABCParameters(self): + print("setDefaultABCParameters is not implemented") + + def setDefaultGaitGeneraterParameters(self): + print("setDefaultGaitGeneraterParameters is not implemented") + + def setDefaultSTParameters(self): + print("setDefaultSTParameters is not implemented") + + def setDefaultServoErrorLimit(self): + pass + + # Basic set of poses + def setJointAngles(self, angle_vector, time=10.0): + if not angle_vector: + print("Not implemented yet") + return + self.seq_svc.setJointAngles(angle_vector, time) + + def resetPose(self): + return [] + + def setResetPose(self, time=5.0): + self.setJointAngles(self.resetPose(), time) + + def initPose(self): + return [0] * len(self.resetPose()) + + def setInitPose(self, time=10.0): + self.setJointAngles(self.initPose(), time) + + def resetManipPose(self): + return [] + + def setResetManipPose(self, time=10.0): + self.setJointAngles(self.resetManipPose(), time) + + def collisionFreeInitPose(self): + return [] + + def setCollisionFreeInitPose(self, time=10.0): + self.setJointAngles(self.collisionFreeInitPose(), time) + + def resetLandingPose(self): + return [] + + def setResetLandingPose(self, time=5.0): + self.setJointAngles(self.resetLandingPose(), time) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py index 5b9d795a..c961a201 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py @@ -2,8 +2,72 @@ from urata_hrpsys_config import * +class URATALEGHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "URATALEG") + + def resetPose(self): + return [0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0, 0.0, 0.0, -0.733038, 1.25664, -0.523599, 0.0] + + def defJointGroups(self): + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5']] + self.Groups = [rleg_group, lleg_group] + + def setDefaultKFParameters(self): + kfp = self.getKFParameters() + kfp.R_angle=1000 + self.setKFParameters(kfp) + + def setDefaultABCParameters(self): + abcp = self.getABCParameters() + abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + abcp.move_base_gain=0.8 + self.setABCParameters(abcp) + + def setDefaultSTParameters(self): + stp = self.getSTParameters() + stp.st_algorithm=OpenHRP.StabilizerService.EEFM + 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.4*5, 20*1.4*5, 1e5]]*2 + stp.eefm_pos_damping_gain=[[3500*10, 3500*10, 3500*1.1*5]]*2 + stp.eefm_rot_time_const=[[1.5, 1.5, 1.5]]*2 + stp.eefm_pos_time_const_support=[[1.5, 1.5, 1.5]]*2 + stp.eefm_wrench_alpha_blending=0.8 + 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 + # mechanical param is => inside 0.055, front 0.13, rear 0.1 + ## kawada ashi + # stp.eefm_leg_inside_margin=0.05 + # stp.eefm_leg_front_margin=0.12 + # stp.eefm_leg_rear_margin=0.09 + ## leptrino ashi + stp.eefm_leg_inside_margin=0.135*0.5 + stp.eefm_leg_front_margin=0.16 + stp.eefm_leg_rear_margin=0.1 + 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])] + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [rleg_vertices, lleg_vertices]) + stp.eefm_zmp_delay_time_const=[0.055, 0.055] + stp.eefm_cogvel_cutoff_freq = 4.25 + stp.eefm_k1=[-1.38077, -1.38077] + stp.eefm_k2=[-0.364652, -0.364652] + stp.eefm_k3=[-0.168538, -0.168538] + self.setSTParameters(stp) + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("URATALEG") + hcf = URATALEGHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py index 90ace93b..e25e76b7 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py @@ -2,8 +2,17 @@ from urata_hrpsys_config import * +class YSTLEGHrpsysConfigurator(URATAHrpsysConfigurator): + def __init__(self): + URATAHrpsysConfigurator.__init__(self, "YSTLEG") + + def defJointGroups(self): + rleg_group = ['rleg', ['RLEG_JOINT0', 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT4_2', 'RLEG_JOINT5']] + lleg_group = ['lleg', ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT4_2', 'LLEG_JOINT5']] + self.Groups = [rleg_group, lleg_group] + if __name__ == '__main__': - hcf = URATAHrpsysConfigurator("YSTLEG") + hcf = YSTLEGHrpsysConfigurator() if len(sys.argv) > 2 : hcf.init(sys.argv[1], sys.argv[2]) elif len(sys.argv) > 1 : From 6c86822290157cd05deb922b41798da90e17d258 Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Fri, 18 Jan 2019 18:10:56 +0900 Subject: [PATCH 2/3] [urata_hrpsys_config.py] Rename setJointAngles in URATAHrpsysConfigurator --- .../urata_hrpsys_config.py | 12 ++++++------ 1 file changed, 6 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 15c9e44a..a0092d35 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 @@ -108,7 +108,7 @@ def setDefaultServoErrorLimit(self): pass # Basic set of poses - def setJointAngles(self, angle_vector, time=10.0): + def setJointAnglesRadian(self, angle_vector, time=10.0): if not angle_vector: print("Not implemented yet") return @@ -118,28 +118,28 @@ def resetPose(self): return [] def setResetPose(self, time=5.0): - self.setJointAngles(self.resetPose(), time) + self.setJointAnglesRadian(self.resetPose(), time) def initPose(self): return [0] * len(self.resetPose()) def setInitPose(self, time=10.0): - self.setJointAngles(self.initPose(), time) + self.setJointAnglesRadian(self.initPose(), time) def resetManipPose(self): return [] def setResetManipPose(self, time=10.0): - self.setJointAngles(self.resetManipPose(), time) + self.setJointAnglesRadian(self.resetManipPose(), time) def collisionFreeInitPose(self): return [] def setCollisionFreeInitPose(self, time=10.0): - self.setJointAngles(self.collisionFreeInitPose(), time) + self.setJointAnglesRadian(self.collisionFreeInitPose(), time) def resetLandingPose(self): return [] def setResetLandingPose(self, time=5.0): - self.setJointAngles(self.resetLandingPose(), time) + self.setJointAnglesRadian(self.resetLandingPose(), time) From dbbf0d58068995edd08e421e6cfbaa23de5e2751 Mon Sep 17 00:00:00 2001 From: Tatsuya Ishikawa Date: Fri, 18 Jan 2019 21:09:56 +0900 Subject: [PATCH 3/3] [*_hrpsys_config.py] Add docstring to describe each class --- .../hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py | 5 +++++ .../jaxon_blue_hrpsys_config.py | 5 +++++ .../hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py | 8 ++++++++ .../jaxon_red_hrpsys_config.py | 6 ++++++ .../hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py | 5 +++++ .../hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py | 5 +++++ .../hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py | 5 +++++ .../hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py | 5 +++++ .../hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py | 5 +++++ 9 files changed, 49 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py index 56617af3..1abb501c 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/chidori_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class CHIDORIHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for CHIDORI configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "CHIDORI") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py index 1e9b8cd8..4f6ba019 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_blue_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class JAXON_BLUEHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for JAXON_BLUE configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "JAXON_BLUE") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py index 09c378d9..ac346a6b 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_hrpsys_config.py @@ -3,6 +3,14 @@ from urata_hrpsys_config import * class JAXONHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for JAXON configuration. + This class has JAXON-dependent code and + common functions for JAXON and JAXON_RED. + Please inherit this class to specify JAXON_RED or + environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "JAXON") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py index d29cf889..5ffb1b3c 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/jaxon_red_hrpsys_config.py @@ -3,6 +3,12 @@ from jaxon_hrpsys_config import * class JAXON_REDHrpsysConfigurator(JAXONHrpsysConfigurator): + """ + Subclass for JAXON_RED configuration. + This class overrides some functions for JAXON_RED. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "JAXON_RED") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py index d9d5dba1..205cc0ee 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/staro_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class STAROHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for STARO configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "STARO") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py index f111150a..83a71450 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/tqleg0_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class TQLEG0HrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for TQLEG0 configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "TQLEG0") 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 a0092d35..1ec648fa 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 @@ -9,6 +9,11 @@ import math class URATAHrpsysConfigurator(HrpsysConfigurator): + """ + Abstract class for JSK robot configuration. + Please inherit this class to specify robot or environmnet-dependent class. + """ + def __init__(self, robotname=""): self.ROBOT_NAME = robotname HrpsysConfigurator.__init__(self) diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py index c961a201..5f8616b8 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urataleg_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class URATALEGHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for URATALEG configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "URATALEG") diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py index e25e76b7..b3dcb198 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py +++ b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/ystleg_hrpsys_config.py @@ -3,6 +3,11 @@ from urata_hrpsys_config import * class YSTLEGHrpsysConfigurator(URATAHrpsysConfigurator): + """ + Subclass for YSTLEG configuration. + Please inherit this class to specify environmnet-dependent class. + """ + def __init__(self): URATAHrpsysConfigurator.__init__(self, "YSTLEG")