diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index e8491f6f..0fd1734e 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -139,19 +139,18 @@ compile_openhrp_model_for_closed_robots(HRP2JSK HRP2JSK_for_OpenHRP3 HRP2JSK --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:RARM_JOINT7 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT2:LARM_JOINT7 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:RARM_JOINT7 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT3:LARM_JOINT7 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:RARM_JOINT7 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT5:LARM_JOINT7 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:RARM_JOINT7 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT2:LARM_JOINT7 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:RARM_JOINT7 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT3:LARM_JOINT7 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:RARM_JOINT7 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT5:LARM_JOINT7 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:RARM_JOINT7 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 CHEST_JOINT1:LARM_JOINT7 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:RARM_JOINT7 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 HEAD_JOINT1:LARM_JOINT7 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:LARM_JOINT7 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:LARM_JOINT7 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:LARM_JOINT7 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:LARM_JOINT7 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:LARM_JOINT7 RARM_JOINT6:WAIST RARM_JOINT7:LARM_JOINT0 RARM_JOINT7:LARM_JOINT2 RARM_JOINT7:LARM_JOINT3 RARM_JOINT7:LARM_JOINT4 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:LARM_JOINT6 RARM_JOINT7:LARM_JOINT7 RARM_JOINT7:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST LARM_JOINT7:WAIST" --conf-file-option "collision_model: convex hull" --conf-file-option "# for PDcontroller" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSK.PDgain.dat" - --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSK.PDgains_sim.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSK.PDgain.dat" --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100" --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)" --conf-file-option "seq_optional_data_dim: 8" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --conf-dt-option "0.004" --simulation-timestep-option "0.004" ) gen_minmax_table_for_closed_robots(HRP2JSK HRP2JSK_for_OpenHRP3 HRP2JSK) -compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_for_OpenHRP3 HRP2JSKNT +compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT --conf-file-option "abc_leg_offset: 0.0,0.105,0.0" --conf-file-option "# 6dof joint version" --conf-file-option "#end_effectors: rleg,RLEG_JOINT5,WAIST,0.035589,-0.01,-0.105,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.035589,0.01,-0.105,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708," @@ -160,24 +159,24 @@ compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_for_OpenHRP3 HRP2JSK --conf-file-option "# Karasawa sole plate + 7dof joint version" --conf-file-option "end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.034,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.034,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708," --conf-file-option "# for ThermoEstimator" - --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3" - --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0" + --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3" + --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0" --conf-file-option "alarm_ratio: 0.75" --conf-file-option "# for CollisionDetector" --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT6 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT6 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT6 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT6:LLEG_JOINT2 RLEG_JOINT6:LLEG_JOINT3 RLEG_JOINT6:LLEG_JOINT5 RLEG_JOINT6:LLEG_JOINT6 RLEG_JOINT6:CHEST_JOINT1 RLEG_JOINT6:RARM_JOINT3 RLEG_JOINT6:RARM_JOINT4 RLEG_JOINT6:RARM_JOINT5 RLEG_JOINT6:RARM_JOINT6 RLEG_JOINT6:LARM_JOINT3 RLEG_JOINT6:LARM_JOINT4 RLEG_JOINT6:LARM_JOINT5 RLEG_JOINT6:LARM_JOINT6 RLEG_JOINT6:WAIST LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT6:CHEST_JOINT1 LLEG_JOINT6:RARM_JOINT3 LLEG_JOINT6:RARM_JOINT4 LLEG_JOINT6:RARM_JOINT5 LLEG_JOINT6:RARM_JOINT6 LLEG_JOINT6:LARM_JOINT3 LLEG_JOINT6:LARM_JOINT4 LLEG_JOINT6:LARM_JOINT5 LLEG_JOINT6:LARM_JOINT6 LLEG_JOINT6:WAIST CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST" --conf-file-option "collision_model: convex hull" --conf-file-option "# for PDcontroller" - --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT.PDgain.dat" - --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT.PDgains_sim.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT.PDgain.dat" --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)" --conf-file-option "seq_optional_data_dim: 8" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "mask_joint_limit: 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" --conf-dt-option "0.004" --simulation-timestep-option "0.004" ) -gen_minmax_table_for_closed_robots(HRP2JSKNT HRP2JSKNT_for_OpenHRP3 HRP2JSKNT) -compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS +gen_minmax_table_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT) +compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS --conf-file-option "abc_leg_offset: 0.0,0.105,0.0" --conf-file-option "# 6dof joint version" --conf-file-option "#end_effectors: rleg,RLEG_JOINT5,WAIST,0.035589,-0.01,-0.105,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.035589,0.01,-0.105,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708," @@ -186,23 +185,23 @@ compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_for_OpenHRP3 HRP2J --conf-file-option "# Karasawa sole plate + 7dof joint version" --conf-file-option "end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.034,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.034,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708," --conf-file-option "# for ThermoEstimator" - --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3" - --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0" + --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3" + --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0" --conf-file-option "alarm_ratio: 0.75" --conf-file-option "# for CollisionDetector" --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT6 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT6 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT6 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT6:LLEG_JOINT2 RLEG_JOINT6:LLEG_JOINT3 RLEG_JOINT6:LLEG_JOINT5 RLEG_JOINT6:LLEG_JOINT6 RLEG_JOINT6:CHEST_JOINT1 RLEG_JOINT6:RARM_JOINT3 RLEG_JOINT6:RARM_JOINT4 RLEG_JOINT6:RARM_JOINT5 RLEG_JOINT6:RARM_JOINT6 RLEG_JOINT6:LARM_JOINT3 RLEG_JOINT6:LARM_JOINT4 RLEG_JOINT6:LARM_JOINT5 RLEG_JOINT6:LARM_JOINT6 RLEG_JOINT6:WAIST LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT6:CHEST_JOINT1 LLEG_JOINT6:RARM_JOINT3 LLEG_JOINT6:RARM_JOINT4 LLEG_JOINT6:RARM_JOINT5 LLEG_JOINT6:RARM_JOINT6 LLEG_JOINT6:LARM_JOINT3 LLEG_JOINT6:LARM_JOINT4 LLEG_JOINT6:LARM_JOINT5 LLEG_JOINT6:LARM_JOINT6 LLEG_JOINT6:WAIST CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST" --conf-file-option "collision_model: convex hull" --conf-file-option "# for PDcontroller" - --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat" - --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgains_sim.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat" --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)" --conf-file-option "seq_optional_data_dim: 8" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-file-option "mask_joint_limit: 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" --conf-dt-option "0.004" --simulation-timestep-option "0.004" ) -gen_minmax_table_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS) +gen_minmax_table_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS) compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W --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" @@ -217,8 +216,8 @@ compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W --conf-file-option "# for CollisionDetector" --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST" --conf-file-option "collision_model: convex hull" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2W.PDgains_sim.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/HRP2W.PDgain.dat" --conf-dt-option "0.004" --simulation-timestep-option "0.004" --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708" @@ -238,29 +237,14 @@ compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G --conf-file-option "# for CollisionDetector" --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST" --conf-file-option "collision_model: convex hull" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2G.PDgains_sim.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/HRP2G.PDgain.dat" --conf-dt-option "0.004" --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(HRP2G HRP2G_for_OpenHRP3 HRP2G) -# compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND HRP2JSKNT_WITH_3HAND -# -a leftarm,CHEST_LINK1,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a leftarm_torso,BODY,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a leftarm_grasp,CHEST_LINK1,LARM_LINK6,0.0,-0.03,-0.17,1.0,0.0,0.0,2.0944,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a rightarm,CHEST_LINK1,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# -a rightarm_torso,BODY,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# -a rightarm_grasp,CHEST_LINK1,RARM_LINK6,0.0,0.03,-0.17,-1.0,0.0,0.0,2.0944,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# ) -# compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND HRP2JSKNTS_WITH_3HAND -# -a leftarm,CHEST_LINK1,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a leftarm_torso,BODY,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a leftarm_grasp,CHEST_LINK1,LARM_LINK6,0.0,-0.03,-0.17,1.0,0.0,0.0,2.0944,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1 -# -a rightarm,CHEST_LINK1,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# -a rightarm_torso,BODY,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# -a rightarm_grasp,CHEST_LINK1,RARM_LINK6,0.0,0.03,-0.17,-1.0,0.0,0.0,2.0944,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1 -# ) + compile_openhrp_model_for_closed_robots(HRP4R HRP4R HRP4R -a leftarm,L_SHOULDER_P_LINK,L_WRIST_R_LINK,0,0,0,0,0,0,1,L_HAND_J0,-1,L_HAND_J1,-1 -a rightarm,R_SHOULDER_P_LINK,R_WRIST_R_LINK,0,0,0,0,0,0,1,R_HAND_J0,1,R_HAND_J1,1 @@ -485,8 +469,8 @@ macro (generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots R endmacro () generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK_for_OpenHRP3 HRP2JSK "--use-robot-hrpsys-config") -generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNT_for_OpenHRP3 HRP2JSKNT "--use-robot-hrpsys-config") -generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS "--use-robot-hrpsys-config") +generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT "--use-robot-hrpsys-config") +generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2W_for_OpenHRP3 HRP2W "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2G_for_OpenHRP3 HRP2G "--use-robot-hrpsys-config") generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP4R HRP4R "--use-unstable-hrpsys-config") @@ -609,20 +593,24 @@ if(${hrp2_models_FOUND} AND multisense_description_FOUND) HRP2G_WH_SENSORS.urdf hrp2g.yaml) endif(${hrp2_models_FOUND} AND multisense_description_FOUND) -# for HRP2JSKNT, HRP2JSKNTS + multisense -if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl) - generate_hand_attached_hrp2_model(HRP2JSKNT) - generate_hand_attached_hrp2_model(HRP2JSKNTS) - run_xacro_for_hand_hrp2_model(HRP2JSKNT) - pkg_check_modules(multisense_description multisense_description QUIET) - if(multisense_description_FOUND) - run_xacro_for_hand_hrp2_model(HRP2JSKNTS) - attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNTS HRP2JSKNTS_WH.urdf - HRP2JSKNTS_WH_SENSORS.urdf hrp2jsknts.yaml) - attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNT HRP2JSKNT_WH.urdf - HRP2JSKNT_WH_SENSORS.urdf hrp2jsknt.yaml) - endif(multisense_description_FOUND) -endif() +# for HRP2JSKNT + multisense +pkg_check_modules(multisense_description multisense_description QUIET) +if(${hrp2_models_FOUND} AND multisense_description_FOUND) + # generate HRP2JSKNT_WH.urdf from HRP2JSKNT.urdf.xacro + set(_model_dir "${PROJECT_SOURCE_DIR}/models/") + add_custom_command(OUTPUT ${_model_dir}/HRP2JSKNT_WH.urdf + COMMAND ${xacro_exe} ${_model_dir}/HRP2JSKNT.urdf.xacro -o ${_model_dir}/HRP2JSKNT_WH.urdf + DEPENDS ${_model_dir}/HRP2JSKNT.urdf.xacro ${_model_dir}/HRP2JSKNT.urdf) + # generate HRP2JSKNT_WH_SENSORS.urdf from HRP2JSKNT_WH.urdf and yaml + attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNT HRP2JSKNT_WH.urdf + HRP2JSKNT_WH_SENSORS.urdf hrp2jsknt.yaml) +endif(${hrp2_models_FOUND} AND multisense_description_FOUND) + +# for HRP2JSKNTS +if(${hrp2_models_FOUND}) + attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNTS HRP2JSKNTS.urdf + HRP2JSKNTS_WH_SENSORS.urdf hrp2jsknts.yaml) +endif(${hrp2_models_FOUND}) # for HIRONXJSK (adding sensor, and fixing some points for gazebo) if(${hrp2_models_FOUND}) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l index 44183f78..c50cd0e9 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l @@ -4,53 +4,66 @@ ) (defmethod hrp2-common-interface - (:define-all-ROSBridge-srv-methods - (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge")) - (send-super :define-all-ROSBridge-srv-methods) - (if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge") - (send-super :define-all-ROSBridge-srv-methods :ros-pkg-name "jsk_hrp2_ros_bridge")) - ) - (:hand-controlmode-vector - (cmode) - (if (find-method self :hrp3handcontrollerservice_setcontrolmodevector) - (send self :hrp3handcontrollerservice_setcontrolmodevector :i_cmode cmode) - (warn ";; :hrp3handcontrollerservice_setcontrolmodevector is not implemented!~%")) - ) (:hand-angle-vector (av &optional (tm 1000)) - (if (find-method self :hrp3handcontrollerservice_setjointangles) - (send self :hrp3handcontrollerservice_setjointangles :jvs av :tm (/ tm 1000.0)) - (warn ";; :hrp3handcontrollerservice_setjointangles is not implemented!~%")) - ) - ;;(:hand-wait-interpolation-raw + (let ((av-all (instantiate float-vector (length (send robot :angle-vector))))) + (dotimes (i 6) + (setelt av-all (position (elt (send robot :limb :rhand :joint-list) i) (send robot :joint-list)) (elt av i)) + (setelt av-all (position (elt (send robot :limb :lhand :joint-list) i) (send robot :joint-list)) (elt av (+ i 6)))) + (send self :angle-vector av-all tm :rhand-controller) + (send self :angle-vector av-all tm :lhand-controller) + av)) (:hand-wait-interpolation () - (if (find-method self :hrp3handcontrollerservice_waitinterpolation) - (send self :hrp3handcontrollerservice_waitinterpolation) - (warn ";; :hrp3handcontrollerservice_waitinterpolation is not implemented!~%")) + (list (send self :wait-interpolation :rhand-controller) + (send self :wait-interpolation :lhand-controller)) ) (:hand-servo-on () - (if (find-method self :hrp3handcontrollerservice_handservoon) - (send self :hrp3handcontrollerservice_handservoon) - (warn ";; :hrp3handcontrollerservice_handservoon is not implemented!~%")) + (warn ";; :hrp3handcontrollerservice_handservoon is not implemented!~%") ) (:hand-servo-off () - (if (find-method self :hrp3handcontrollerservice_handservooff) - (send self :hrp3handcontrollerservice_handservooff) - (warn ";; :hrp3handcontrollerservice_handservooff is not implemented!~%")) + (warn ";; :hrp3handcontrollerservice_handservooff is not implemented!~%") ) (:hand-joint-calib () - (if (find-method self :hrp3handcontrollerservice_handjointcalib) - (send self :hrp3handcontrollerservice_handjointcalib) - (warn ";; :hrp3handcontrollerservice_handjointcalib is not implemented!~%")) + (warn ";; :hrp3handcontrollerservice_handjointcalib is not implemented!~%") ) (:hand-state () - (if (find-method self :hrp3handcontrollerservice_getRobotState) - (send self :hrp3handcontrollerservice_getRobotState) - (warn ";; :hrp3handcontrollerservice_getRobotState is not implemented!~%")) + (send self :state) + (let ((reference-all (send self :reference-vector)) + (reference (instantiate float-vector 12))) + (dotimes (i 6) + (setelt reference i (elt reference-all (position (elt (send robot :limb :rhand :joint-list) i) (send robot :joint-list)))) + (setelt reference (+ i 6) (elt reference-all (position (elt (send robot :limb :lhand :joint-list) i) (send robot :joint-list))))) + (instance hrp3hand-state :init + :reference reference + :potentio (send robot :hand-angle-vector) + ) + ) ) ) + +(defclass hrp3hand-state + :super propertied-object + :slots (_reference _potentio) + ) + +(defmethod hrp3hand-state + (:init + (&key + ((:reference __reference) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:potentio __potentio) (make-array 0 :initial-element 0.0 :element-type :float)) + ) + (setq _reference __reference) + (setq _potentio __potentio) + self) + (:reference + (&optional __reference) + (if __reference (setq _reference __reference)) _reference) + (:potentio + (&optional __potentio) + (if __potentio (setq _potentio __potentio)) _potentio) + ) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l index 1256f3ea..1b59ab27 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l @@ -3,8 +3,6 @@ (when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l")) (require :hrp2jsknt-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l")) (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l") -(if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge") - (ros::load-ros-manifest "jsk_hrp2_ros_bridge")) (defclass hrp2jsknt-interface :super hrp2-common-interface @@ -14,7 +12,7 @@ (prog1 (send-super* :init :robot hrp2jsknt-robot args) ;; add controller - (dolist (limb '(:rarm :larm :rleg :lleg :head :torso)) + (dolist (limb '(:rarm :larm :rleg :lleg :head :torso :rhand :lhand)) (send self :def-limb-controller-method limb) (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t)))) ) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l index 2d8a9a1c..0081fcf8 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l @@ -1,8 +1,6 @@ -(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l") +(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l") (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l") -(unless (assoc :init-org (send hrp2jsknt-robot :methods)) - (rplaca (assoc :init (send hrp2jsknt-robot :methods)) :init-org)) (eval `(defmethod hrp2jsknt-robot ,@(get-hrp2-with-hand-class-methods))) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l index e0cc483c..91dde7a9 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l @@ -3,8 +3,6 @@ (when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l")) (require :hrp2jsknts-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l")) (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l") -(if (ros::resolve-ros-path "package://jsk_hrp2_ros_bridge") - (ros::load-ros-manifest "jsk_hrp2_ros_bridge")) (defclass hrp2jsknts-interface :super hrp2-common-interface @@ -14,7 +12,7 @@ (prog1 (send-super* :init :robot hrp2jsknts-robot args) ;; add controller - (dolist (limb '(:rarm :larm :rleg :lleg :head :torso)) + (dolist (limb '(:rarm :larm :rleg :lleg :head :torso :rhand :lhand)) (send self :def-limb-controller-method limb) (send self :add-controller (read-from-string (format nil "~A-controller" limb)) :joint-enable-check t :create-actions t)))) ) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l index 9bf513d5..dd3ed308 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l @@ -1,8 +1,6 @@ -(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l") +(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l") (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l") -(unless (assoc :init-org (send hrp2jsknts-robot :methods)) - (rplaca (assoc :init (send hrp2jsknts-robot :methods)) :init-org)) (eval `(defmethod hrp2jsknts-robot ,@(get-hrp2-with-hand-class-methods))) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l index 53cd1102..f5e25728 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l @@ -1,120 +1,50 @@ -(load "package://hrpsys_ros_bridge_tutorials/models/hrp3hand_l.l") -(load "package://hrpsys_ros_bridge_tutorials/models/hrp3hand_r.l") - -(defun get-hrp3hand-class-methods () - '( - ;; poses - (:open-pose () - (send self :angle-vector (float-vector 0 60 -10 30 0 0))) - (:preclose-pose () - (send self :angle-vector (float-vector 40 60 -10 35 10 90))) - (:close-pose () - (send self :angle-vector (float-vector 65 0 30 50 90 90))) - (:reset-pose () - (send self :angle-vector (float-vector 0 0 0 0 0 0))) - ;; - (:standard-pose () ;; PoS - (send self :angle-vector (float-vector 20 90 0 10 -20 -20))) - (:hook-pose () ;; PoH - (send self :angle-vector (float-vector 90 90 0 10 -20 -20))) - (:index-pose () ;; PoI - (send self :angle-vector (float-vector 60 90 0 70 -20 -20))) - (:extension-pose () ;; PoE - (send self :angle-vector (float-vector 90 30 0 10 -20 -20))) - (:distal-pose () ;; PoD - (send self :angle-vector (float-vector 50 60 -20 10 20 40))) - (:hook-pose2 () - (send self :angle-vector (float-vector 90 70 0 10 -20 -40))) - (:distal-pose2 () - (send self :angle-vector (float-vector 90 90 -20 10 20 60))) - (:grasp-pose () - (send self :angle-vector (float-vector 77.9709 -11.4732 8.28742 -16.3569 106.185 86.0974))) - (:index-avoid-extention-pose2 - () - (send self :angle-vector (float-vector 90.0 -30.0 -10.0 10.0 -40.0 -40.0))) - ;; - ;; def index avoid methods - ;; index avoid methods -> does not use index finger to avoid overload ;; index finger maximum torque is very low. - ;; User can customize index finger joint angles by using index-angle-vector argument - ;; Currently we prepare index avoid method for :hook-pose, :reset-pose, :hook-pose2, :distal-pose2 - (:def-index-avoid-pose-methods - () - ;; - ;; - (dolist (pose (list :hook-pose :reset-pose :hook-pose2 :distal-pose2 :grasp-pose)) - (eval - `(defmethod ,(send (class self) :name) - (,(read-from-string (format nil ":index-avoid-~A" (string-downcase pose))) - (&optional (index-angle-vector (float-vector -10 -10 -40))) - (send self ,pose) - (send self :f1-1r :joint-angle (elt index-angle-vector 0)) - (send self :f1-1p :joint-angle (elt index-angle-vector 1)) - (send self :f1-2r :joint-angle (elt index-angle-vector 2)) - (send self :angle-vector) - )))) - ) - ;; - (:f1-1r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-1r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - (:f1-1p (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-1p" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - (:f1-2r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f1-2r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - (:f2-2r (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-f2-2r" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - (:t-1y (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-t-1y" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - (:t-1p (&rest args) (forward-message-to (send self (read-from-string (format nil "~A-t-1p" (find-if #'(lambda (x) (send self x)) '(:rarm :larm))))) args)) - ) - ) - (defun get-hrp2-with-hand-class-methods () '( - (:init - (&rest args) - (prog1 - (send* self :init-org args) - (send self :put :lhand-model (instance hrp3hand_l-robot :init)) - (send self :put :rhand-model (instance hrp3hand_r-robot :init)) - (dolist (h (list (send self :get :lhand-model) (send self :get :rhand-model))) - (send h :def-index-avoid-pose-methods)) - (mapcar #'(lambda (l lm) - (send (send self :get lm) :newcoords - (send (send self l :end-coords :parent) :copy-worldcoords)) - (send (send self l :end-coords :parent) :assoc (send self :get lm))) - '(:larm :rarm) '(:lhand-model :rhand-model)) - (setq bodies (append bodies - (send (send self :get :lhand-model) :bodies) - (send (send self :get :rhand-model) :bodies))) - )) (:hand (arm &rest args) - (let (hr ret) - (case arm - (:larm - (setq hr (send self :get :lhand-model)) - (setq ret (forward-message-to hr args))) - (:rarm - (setq hr (send self :get :rhand-model)) - (setq ret (forward-message-to hr args))) - ((:both :arms) - (setq hr (send self :get :lhand-model)) - (push (forward-message-to hr args) ret) - (setq hr (send self :get :rhand-model)) - (push (forward-message-to hr args) ret)) - ) - ret)) + (case arm + (:rarm (send* self :limb :rhand args)) + (:larm (send* self :limb :lhand args)) + ((:both :arms) (list (send* self :limb :rhand args) (send* self :limb :lhand args))))) + (:limb + (limb &optional method &rest args) + (if (member limb '(:rhand :lhand)) + (let ((av (case method + (:open-pose (float-vector 0 60 -10 30 0 0)) + (:preclose-pose (float-vector 40 60 -10 35 10 90)) + (:close-pose (float-vector 65 0 30 50 90 90)) + (:reset-pose (float-vector 0 0 0 0 0 0)) + ;; + (:standard-pose (float-vector 20 90 0 10 -20 -20)) + (:hook-pose (float-vector 90 90 0 10 -20 -20)) + (:index-pose (float-vector 60 90 0 70 -20 -20)) + (:extension-pose (float-vector 90 30 0 10 -20 -20)) + (:distal-pose (float-vector 50 60 -20 10 20 40)) + (:hook-pose2 (float-vector 90 70 0 10 -20 -40)) + (:distal-pose2 (float-vector 90 90 -20 10 20 60)) + (:grasp-pose (float-vector 77.9709 -11.4732 8.28742 -16.3569 106.185 86.0974)) + (:index-avoid-extention-pose2 (float-vector 90.0 -30.0 -10.0 10.0 -40.0 -40.0)) + (:index-avoid-hook-pose (float-vector 90 90 -10 -10 -40 -20)) + (:index-avoid-reset-pose (float-vector 0 0 -10 -10 -40 0)) + (:index-avoid-hook-pose2 (float-vector 90 70 -10 -10 -40 -40)) + (:index-avoid-distal-pose (float-vector 50 60 -10 -10 -40 40)) + (:index-avoid-grasp-pose (float-vector 77.9709 -11.4732 -10 -10 -40 86.0974))))) + (if av + (send-super :limb limb :angle-vector av) + (send-super* :limb limb method args))) + (send-super* :limb limb method args))) (:hand-angle-vector (&optional (av)) (when av - (send self :hand :rarm :angle-vector (subseq av 0 6)) - (send self :hand :larm :angle-vector (subseq av 6)) + (send self :limb :rhand :angle-vector (subseq av 0 6)) + (send self :limb :lhand :angle-vector (subseq av 6)) ) (concatenate float-vector - (send self :hand :rarm :angle-vector) - (send self :hand :larm :angle-vector)) + (send self :limb :rhand :angle-vector) + (send self :limb :lhand :angle-vector)) + ) + (:collision-check-pairs + (&key ((:links ls) (set-difference (cons (car links) (all-child-links (car links))) + (append (send self :limb :rhand :links) (send self :limb :lhand :links))))) + (send-super :collision-check-pairs :links ls)) ) - ) ) - -(eval - `(defmethod hrp3hand_l-robot - ,@(get-hrp3hand-class-methods))) - -(eval - `(defmethod hrp3hand_r-robot - ,@(get-hrp3hand-class-methods))) diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgain.dat new file mode 100644 index 00000000..b4c29d90 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgain.dat @@ -0,0 +1,23 @@ +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgains_sim.dat new file mode 100644 index 00000000..b474708b --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2G.PDgains_sim.dat @@ -0,0 +1,23 @@ +3200 20 +10900 42 + +330 2.5 +330 2.5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2G_withoutGrippers.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2G_withoutGrippers.PDgain.dat new file mode 100644 index 00000000..a906d724 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2G_withoutGrippers.PDgain.dat @@ -0,0 +1,23 @@ +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +0 0 0 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +0 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat index fe9ae702..e01d0f45 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgain.dat @@ -1,32 +1,37 @@ -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 +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 + +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 + +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgains_sim.dat new file mode 100644 index 00000000..2efb0fba --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgains_sim.dat @@ -0,0 +1,37 @@ +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 + +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 + +3200 20 +10900 42 + +330 2.5 +330 2.5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 \ 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..4b941e35 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat @@ -1,34 +1,53 @@ -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 +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 +2.5 0.4 30 + +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 +2.5 0.4 30 + +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 + +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgains_sim.dat new file mode 100644 index 00000000..b9276513 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgains_sim.dat @@ -0,0 +1,53 @@ +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 +120 1 + +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 +120 1 + +3200 20 +10900 42 + +330 2.5 +330 2.5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +0 0 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +0 0 + +50 10 +50 10 +50 10 +50 10 +50 10 +50 10 + +50 10 +50 10 +50 10 +50 10 +50 10 +50 10 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro index 42ea3364..c84abb41 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.urdf.xacro @@ -1,7 +1,5 @@ - - - + diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat index 6fbf55ef..4b941e35 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat @@ -1,34 +1,53 @@ -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 +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 +2.5 0.4 30 + +2 0.4 20 +5 0.4 40 +2.5 0.4 30 +4.5 0.4 45 +2 0.4 20 +5 0.4 50 +2.5 0.4 30 + +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 + +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 +1 0.4 0 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgains_sim.dat new file mode 100644 index 00000000..b9276513 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgains_sim.dat @@ -0,0 +1,53 @@ +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 +120 1 + +900 25 +4300 12.5 +3300 17 +4100 11 +2420 25 +750 10 +120 1 + +3200 20 +10900 42 + +330 2.5 +330 2.5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +0 0 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +0 0 + +50 10 +50 10 +50 10 +50 10 +50 10 +50 10 + +50 10 +50 10 +50 10 +50 10 +50 10 +50 10 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.urdf.xacro b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.urdf.xacro deleted file mode 100644 index 4aa4968d..00000000 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.urdf.xacro +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgain.dat new file mode 100644 index 00000000..b4c29d90 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgain.dat @@ -0,0 +1,23 @@ +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +1 0.4 10 diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgains_sim.dat b/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgains_sim.dat new file mode 100644 index 00000000..b474708b --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2W.PDgains_sim.dat @@ -0,0 +1,23 @@ +3200 20 +10900 42 + +330 2.5 +330 2.5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 + +3600 6.7 +3700 6.7 +430 10 +4350 5 +700 5 +460 10 +1870 10 +700 5 \ No newline at end of file diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2W_withoutGrippers.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2W_withoutGrippers.PDgain.dat new file mode 100644 index 00000000..a906d724 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2W_withoutGrippers.PDgain.dat @@ -0,0 +1,23 @@ +2.5 0.4 25 +1.2 0.4 12 + +4.0 0.4 40 +4.0 0.4 40 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +0 0 0 + +1.5 0.4 15 +1.5 0.4 15 +1 0.4 10 +2 0.4 20 +2 0.4 20 +1 0.4 10 +1 0.4 10 +0 0 0 diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml index fd338a87..39b76645 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt.yaml @@ -1,6 +1,7 @@ ## ## - collada_joint_name : euslisp_joint_name (start with :) ## +limbs: [rleg, lleg, torso, head, rarm, larm, rhand, lhand] rleg: - RLEG_JOINT0 : rleg-crotch-y @@ -19,8 +20,8 @@ lleg: - LLEG_JOINT5 : lleg-ankle-r - LLEG_JOINT6 : lleg-toe-p torso: - - CHEST_JOINT0 : torso-waist-y - - CHEST_JOINT1 : torso-waist-p + - CHEST_JOINT0 : torso-waist-p + - CHEST_JOINT1 : torso-waist-y head: - HEAD_JOINT0 : head-neck-y - HEAD_JOINT1 : head-neck-p @@ -42,6 +43,20 @@ larm: - LARM_JOINT5 : larm-wrist-r - LARM_JOINT6 : larm-wrist-p - LARM_JOINT7 : larm-thumb-r +rhand: + - R_THUMBCM_Y : rarm-t-1y + - R_THUMBCM_P : rarm-t-1p + - R_INDEXMP_R : rarm-f1-1r + - R_INDEXMP_P : rarm-f1-1p + - R_INDEXPIP_R : rarm-f1-2r + - R_MIDDLEPIP_R : rarm-f2-2r +lhand: + - L_THUMBCM_Y : larm-t-1y + - L_THUMBCM_P : larm-t-1p + - L_INDEXMP_R : larm-f1-1r + - L_INDEXMP_P : larm-f1-1p + - L_INDEXPIP_R : larm-f1-2r + - L_MIDDLEPIP_R : larm-f2-2r ## ## end-coords @@ -140,13 +155,17 @@ angle-vector: 0.0, 0.0, 0.0, 0.0, 10.0, -10.0, 0.0, -25.0, 0.0, 0.0, -10.0, 15.0, - 10.0, 10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.0] + 10.0, 10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.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] reset-manip-pose : [0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, 0.0, 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] + 50.0, 30.0, 10.0, -120.0, 25.0, 5.0, -20.0, -60.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] # for gazebo simulation replace_xmls: - match_rule: diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml deleted file mode 100644 index 742b7600..00000000 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml +++ /dev/null @@ -1,82 +0,0 @@ -## -## - collada_joint_name : euslisp_joint_name (start with :) -## - -rleg: - - RLEG_JOINT0 : rleg-crotch-y - - RLEG_JOINT1 : rleg-crotch-r - - RLEG_JOINT2 : rleg-crotch-p - - RLEG_JOINT3 : rleg-knee-p - - RLEG_JOINT4 : rleg-ankle-p - - RLEG_JOINT5 : rleg-ankle-r -lleg: - - LLEG_JOINT0 : lleg-crotch-y - - LLEG_JOINT1 : lleg-crotch-r - - LLEG_JOINT2 : lleg-crotch-p - - LLEG_JOINT3 : lleg-knee-p - - LLEG_JOINT4 : lleg-ankle-p - - LLEG_JOINT5 : lleg-ankle-r -torso: - - CHEST_JOINT0 : torso-waist-p - - CHEST_JOINT1 : torso-waist-y -head: - - HEAD_JOINT0 : head-neck-y - - HEAD_JOINT1 : head-neck-p -rarm: - - RARM_JOINT0 : rarm-shoulder-p - - RARM_JOINT1 : rarm-shoulder-r - - RARM_JOINT2 : rarm-shoulder-y - - RARM_JOINT3 : rarm-elbow-p - - RARM_JOINT4 : rarm-wrist-y - - RARM_JOINT5 : rarm-wrist-r - - RARM_JOINT6 : rarm-wrist-p - - R_THUMBCM_Y : rarm-t-1y - - R_THUMBCM_P : rarm-t-1p - - R_INDEXMP_R : rarm-f1-1r - - R_INDEXMP_P : rarm-f1-1p - - R_INDEXPIP_R : rarm-f1-2p - - R_MIDDLEPIP_R : rarm-f2-2r -larm: - - LARM_JOINT0 : larm-shoulder-p - - LARM_JOINT1 : larm-shoulder-r - - LARM_JOINT2 : larm-shoulder-y - - LARM_JOINT3 : larm-elbow-p - - LARM_JOINT4 : larm-wrist-y - - LARM_JOINT5 : larm-wrist-r - - LARM_JOINT6 : larm-wrist-p - - L_THUMBCM_Y : larm-t-1y - - L_THUMBCM_P : larm-t-1p - - L_INDEXMP_R : larm-f1-1r - - L_INDEXMP_P : larm-f1-1p - - L_INDEXPIP_R : larm-f1-2p - - L_MIDDLEPIP_R : larm-f2-2r - -## -## end-coords -## -rleg-end-coords: - translate : [0.0, 0, -0.091849] -lleg-end-coords: - translate : [0.0, 0, -0.091849] -rarm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] -larm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] -head-end-coords: - translate : [0.085, 0, 0.0] - rotate : [0, 1, 0, 90] - -## -## reset-pose -## -angle-vector: - reset-pose : [0.0, -1, -20, 40, -20, 1, - 0.0, 1, -20, 40, -20, -1, - 0.0, 0.0, - 0.0, 0.0, - 20.0, -10.0, 0.0, -30.0, 0.0, 0.0, 0.0, - 0, 0, 0, 0, 0, 0, - 20.0, 10.0, 0.0, -30.0, 0.0, 0.0, 0.0, - 0, 0, 0, 0, 0, 0] diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml index fa043925..06695a2a 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml @@ -1,6 +1,7 @@ ## ## - collada_joint_name : euslisp_joint_name (start with :) ## +limbs: [rleg, lleg, torso, head, rarm, larm, rhand, lhand] rleg: - RLEG_JOINT0 : rleg-crotch-y @@ -42,6 +43,20 @@ larm: - LARM_JOINT5 : larm-wrist-r - LARM_JOINT6 : larm-wrist-p - LARM_JOINT7 : larm-thumb-r +rhand: + - R_THUMBCM_Y : rarm-t-1y + - R_THUMBCM_P : rarm-t-1p + - R_INDEXMP_R : rarm-f1-1r + - R_INDEXMP_P : rarm-f1-1p + - R_INDEXPIP_R : rarm-f1-2r + - R_MIDDLEPIP_R : rarm-f2-2r +lhand: + - L_THUMBCM_Y : larm-t-1y + - L_THUMBCM_P : larm-t-1p + - L_INDEXMP_R : larm-f1-1r + - L_INDEXMP_P : larm-f1-1p + - L_INDEXPIP_R : larm-f1-2r + - L_MIDDLEPIP_R : larm-f2-2r ## ## end-coords @@ -146,13 +161,17 @@ angle-vector: 0.0, 0.0, 0.0, 0.0, 10.0, -10.0, 0.0, -25.0, 0.0, 0.0, -10.0, 15.0, - 10.0, 10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.0] + 10.0, 10.0, 0.0, -25.0, 0.0, 0.0, -10.0, -15.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] reset-manip-pose : [0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, 0.0, 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] + 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, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # for gazebo simulation replace_xmls: - match_rule: diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml deleted file mode 100644 index 742b7600..00000000 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml +++ /dev/null @@ -1,82 +0,0 @@ -## -## - collada_joint_name : euslisp_joint_name (start with :) -## - -rleg: - - RLEG_JOINT0 : rleg-crotch-y - - RLEG_JOINT1 : rleg-crotch-r - - RLEG_JOINT2 : rleg-crotch-p - - RLEG_JOINT3 : rleg-knee-p - - RLEG_JOINT4 : rleg-ankle-p - - RLEG_JOINT5 : rleg-ankle-r -lleg: - - LLEG_JOINT0 : lleg-crotch-y - - LLEG_JOINT1 : lleg-crotch-r - - LLEG_JOINT2 : lleg-crotch-p - - LLEG_JOINT3 : lleg-knee-p - - LLEG_JOINT4 : lleg-ankle-p - - LLEG_JOINT5 : lleg-ankle-r -torso: - - CHEST_JOINT0 : torso-waist-p - - CHEST_JOINT1 : torso-waist-y -head: - - HEAD_JOINT0 : head-neck-y - - HEAD_JOINT1 : head-neck-p -rarm: - - RARM_JOINT0 : rarm-shoulder-p - - RARM_JOINT1 : rarm-shoulder-r - - RARM_JOINT2 : rarm-shoulder-y - - RARM_JOINT3 : rarm-elbow-p - - RARM_JOINT4 : rarm-wrist-y - - RARM_JOINT5 : rarm-wrist-r - - RARM_JOINT6 : rarm-wrist-p - - R_THUMBCM_Y : rarm-t-1y - - R_THUMBCM_P : rarm-t-1p - - R_INDEXMP_R : rarm-f1-1r - - R_INDEXMP_P : rarm-f1-1p - - R_INDEXPIP_R : rarm-f1-2p - - R_MIDDLEPIP_R : rarm-f2-2r -larm: - - LARM_JOINT0 : larm-shoulder-p - - LARM_JOINT1 : larm-shoulder-r - - LARM_JOINT2 : larm-shoulder-y - - LARM_JOINT3 : larm-elbow-p - - LARM_JOINT4 : larm-wrist-y - - LARM_JOINT5 : larm-wrist-r - - LARM_JOINT6 : larm-wrist-p - - L_THUMBCM_Y : larm-t-1y - - L_THUMBCM_P : larm-t-1p - - L_INDEXMP_R : larm-f1-1r - - L_INDEXMP_P : larm-f1-1p - - L_INDEXPIP_R : larm-f1-2p - - L_MIDDLEPIP_R : larm-f2-2r - -## -## end-coords -## -rleg-end-coords: - translate : [0.0, 0, -0.091849] -lleg-end-coords: - translate : [0.0, 0, -0.091849] -rarm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] -larm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] -head-end-coords: - translate : [0.085, 0, 0.0] - rotate : [0, 1, 0, 90] - -## -## reset-pose -## -angle-vector: - reset-pose : [0.0, -1, -20, 40, -20, 1, - 0.0, 1, -20, 40, -20, -1, - 0.0, 0.0, - 0.0, 0.0, - 20.0, -10.0, 0.0, -30.0, 0.0, 0.0, 0.0, - 0, 0, 0, 0, 0, 0, - 20.0, 10.0, 0.0, -30.0, 0.0, 0.0, 0.0, - 0, 0, 0, 0, 0, 0] 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..cebe6107 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 @@ -27,8 +27,10 @@ def defJointGroups (self): head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']] 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']] + rhand_group = ['rhand', ['R_THUMBCM_Y', 'R_THUMBCM_P', 'R_INDEXMP_R', 'R_INDEXMP_P', 'R_INDEXPIP_R', 'R_MIDDLEPIP_R']] + lhand_group = ['lhand', ['L_THUMBCM_Y', 'L_THUMBCM_P', 'L_INDEXMP_R', 'L_INDEXMP_P', 'L_INDEXPIP_R', 'L_MIDDLEPIP_R']] if self.ROBOT_NAME == "HRP2JSKNT" or self.ROBOT_NAME == "HRP2JSKNTS": - self.Groups = [rleg_7dof_group, lleg_7dof_group, torso_group, head_group, rarm_group, larm_group] + self.Groups = [rleg_7dof_group, lleg_7dof_group, torso_group, head_group, rarm_group, larm_group, rhand_group, lhand_group] elif self.ROBOT_NAME == "HRP2JSK": self.Groups = [rleg_6dof_group, lleg_6dof_group, torso_group, head_group, rarm_group, larm_group] else: # HRP2W, HRP2G @@ -36,7 +38,7 @@ def defJointGroups (self): def hrp2ResetPose (self): if self.ROBOT_NAME.find("HRP2JSKNT") != -1: - return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.174533, -0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, 0.261799, 0.174533, 0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, -0.261799] + return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.174533, -0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, 0.261799, 0.174533, 0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, -0.261799] + [0.0]*12 elif self.ROBOT_NAME.find("HRP2JSK") != -1: return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, 0.0, 0.174533, -0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, 0.261799, 0.174533, 0.174533, 0.0, -0.436332, 0.0, 0.0, -0.174533, -0.261799] else: @@ -44,7 +46,7 @@ def hrp2ResetPose (self): def hrp2ResetManipPose (self): if self.ROBOT_NAME.find("HRP2JSKNT") != -1: - return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 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] + return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 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] + [0.0]*12 elif self.ROBOT_NAME.find("HRP2JSK") != -1: return [0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 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] else: @@ -371,6 +373,24 @@ def setResetManipPose(self): def setInitPose(self): self.seq_svc.setJointAngles(self.hrp2InitPose(), 5.0) + def hrp3HandResetPose(self): + self.seq_svc.setJointAnglesOfGroup("rhand", [0, 0, 0, 0, 0, 0], 2) + self.seq_svc.setJointAnglesOfGroup("lhand", [0, 0, 0, 0, 0, 0], 2) + self.seq_svc.waitInterpolationOfGroup("rhand") + self.seq_svc.waitInterpolationOfGroup("lhand") + + def hrp3HandGraspPose(self): + self.seq_svc.setJointAnglesOfGroup("rhand", [1.36085, -0.200245, 0.144643, 0.0, 1.85328, 1.50268], 2) + self.seq_svc.setJointAnglesOfGroup("lhand", [1.36085, -0.200245, 0.144643, 0.0, 1.85328, 1.50268], 2) + self.seq_svc.waitInterpolationOfGroup("rhand") + self.seq_svc.waitInterpolationOfGroup("lhand") + + def hrp3HandHookPose(self): + self.seq_svc.setJointAnglesOfGroup("rhand", [1.5708, 1.5708, 0.0, 0.174533, -0.349066, -0.349066], 2) + self.seq_svc.setJointAnglesOfGroup("lhand", [1.5708, 1.5708, 0.0, 0.174533, -0.349066, -0.349066], 2) + self.seq_svc.waitInterpolationOfGroup("rhand") + self.seq_svc.waitInterpolationOfGroup("lhand") + def loadForceMomentOffsetFile (self): import rospkg if self.ROBOT_NAME == "HRP2JSKNT":