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_gazebo_tutorials/config/HRP2JSKNT.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml index be1d0ede..fecbb7b7 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: 2800.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} @@ -94,18 +82,6 @@ hrpsys_gazebo_configuration: RLEG_JOINT4: {p: 1400.0, d: 1.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT5: {p: 3500.0, d: 3.0, 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 72613d0d..76808025 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: 2800.0, d: 3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} @@ -94,18 +82,6 @@ hrpsys_gazebo_configuration: RLEG_JOINT4: {p: 1400.0, d: 1.5, i: 0.0, i_clamp: 0.0, p_v: 350.0} RLEG_JOINT5: {p: 3500.0, d: 3.0, 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/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index efcbea82..3536c0ea 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -223,7 +223,7 @@ compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W --simulation-timestep-option "0.004" --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708" ) -gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) +# gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W) compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G --conf-file-option "end_effectors: rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708" --conf-file-option "# for ThermoEstimator" @@ -426,7 +426,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" ) 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/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 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/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 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/models/jaxon_red.yaml b/hrpsys_ros_bridge_tutorials/models/jaxon_red.yaml index 9a6150d7..9da0a9b1 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/hrp2_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py index 3bf0d267..e2fa4403 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" @@ -50,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()) @@ -76,10 +141,11 @@ 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.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -97,7 +163,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 @@ -112,17 +178,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] @@ -136,18 +202,20 @@ 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.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) # 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=1.0 + gg.default_double_support_ratio=0.2 #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 @@ -156,12 +224,30 @@ 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.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.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 = 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 = 1.0 + gg.overwritable_max_time = 2.0 + gg.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] @@ -175,10 +261,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] @@ -194,7 +281,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 @@ -209,17 +296,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] @@ -233,18 +320,20 @@ 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.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) # 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 @@ -253,12 +342,29 @@ 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.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.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.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] @@ -272,10 +378,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.EEFMQPCOP # eefm st params stp.eefm_body_attitude_control_gain=[1.5, 1.5] stp.eefm_body_attitude_control_time_const=[10000, 10000] @@ -291,7 +398,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 @@ -306,17 +413,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] @@ -330,18 +437,20 @@ 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.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) # 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 @@ -350,11 +459,28 @@ 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; 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_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.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.is_interpolate_zmp_in_double = True self.abc_svc.setGaitGeneratorParam(gg) # Estop esp=self.es_svc.getEmergencyStopperParam()[1] @@ -368,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) @@ -382,6 +511,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) 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 3e898b68..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 @@ -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() @@ -189,124 +189,176 @@ 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.move_base_gain=0.8 + 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) # 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) - # 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] + # 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] 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 = [[60, 60, 1e5], + astp.eefm_rot_damping_gain = [[60, 60, 1e5], [60, 60, 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 = [[33600, 33600, 9000], + astp.eefm_pos_damping_gain = [[33600, 33600, 9000], [33600, 33600, 9000], [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 + # 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]] + # 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]] + 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=[[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 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.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 - 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_FORCE_PLATE": - 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 + 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 elif foot == "LEPTRINO_FORCE_SENSOR": ## Leptrino force sensor foot : mechanical param is => inside 0.07, front 0.12, rear 0.11 - stp.eefm_leg_inside_margin=0.065 - stp.eefm_leg_outside_margin=0.065 - stp.eefm_leg_front_margin=0.115 - stp.eefm_leg_rear_margin=0.105 - 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.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]), + 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 - 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) + 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] + 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.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,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]} + 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.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.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE + 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 + 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'] @@ -317,102 +369,137 @@ 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] + esp.default_retrieve_duration=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.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.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'] @@ -423,8 +510,9 @@ 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) def setStAbcParametersURATALEG (self): @@ -485,8 +573,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.0, 0.0, 0.0], [0.0, 0.0, 0.0]]; - abcp.move_base_gain=0.8 + 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) # kf setting kfp=self.kf_svc.getKalmanFilterParam()[1] @@ -498,8 +587,8 @@ 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_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_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 stp.eefm_pos_time_const_support=[[1.5/1.1, 1.5/1.1, 1.5/1.1]]*2 @@ -515,6 +604,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 @@ -522,10 +612,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 @@ -555,6 +645,84 @@ def setStAbcParametersCHIDORI (self): stp.eefm_k2=[-0.368975,-0.368975] stp.eefm_k3=[-0.169915,-0.169915] self.st_svc.setParameter(stp) + # 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] + 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 = [[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 + 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=[[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 + 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] + stp.detection_time_to_air=5.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.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 + # 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.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* + 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] #gg.stride_parameter=[0.1,0.05,10.0] @@ -563,18 +731,32 @@ 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 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.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] + 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):