From 631cb696d4a4082489f7b61b757b54ea0fedf313 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Mon, 19 Apr 2021 00:39:16 +0900 Subject: [PATCH 1/4] hrp2_with_3hand --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 93 ++++- .../euslisp/hrp2-common-interface.l | 137 ++++++-- .../euslisp/hrp2jsknt-interface.l | 6 +- .../euslisp/hrp2jsknt-utils.l | 6 +- .../euslisp/hrp2jsknts-interface.l | 6 +- .../euslisp/hrp2jsknts-utils.l | 6 +- .../euslisp/hrp3hand-utils.l | 144 ++------ .../models/HRP2JSKNTS_WITH_3HAND.PDgain.dat | 46 +++ .../models/HRP2JSKNT_WITH_3HAND.PDgain.dat | 46 +++ .../models/hrp2jsknt_with_3hand.yaml | 316 +++++++++++++++-- .../models/hrp2jsknts_with_3hand.yaml | 326 ++++++++++++++++-- .../hrp2_hrpsys_config.py | 26 +- 12 files changed, 934 insertions(+), 224 deletions(-) create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index e8491f6f..cbac2721 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -245,22 +245,75 @@ compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G --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(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT_WITH_3HAND + --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," + --conf-file-option "# Original foot + 7dof joint version" + --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,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 "# 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, 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,100,100,100,100,100,100,100,100,100,100,100,100" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT_WITH_3HAND.PDgain.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --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" + --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" + --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-dt-option "0.004" + --simulation-timestep-option "0.004" + -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 + ) +#gen_minmax_table_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT_WITH_3HAND) + +compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS_WITH_3HAND + --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," + --conf-file-option "# Original foot + 7dof joint version" + --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,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 "# 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, 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,100,100,100,100,100,100,100,100,100,100,100,100" + --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat" + --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --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" + --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" + --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" + --conf-dt-option "0.004" + --simulation-timestep-option "0.004" + -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 + ) +#gen_minmax_table_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS_WITH_3HAND) + 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 @@ -624,6 +677,14 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl) endif(multisense_description_FOUND) endif() +# for HRP2JSKNT_WITH_3HAND, HRP2JSKNTS_WITH_3HAND (adding sensor, and fixing some points for gazebo) +if(${hrp2_models_FOUND}) + attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNT HRP2JSKNT_WITH_3HAND.urdf + HRP2JSKNT_WITH_3HAND_SENSORS.urdf hrp2jsknt_with_3hand.yaml) + attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND.urdf + HRP2JSKNTS_WITH_3HAND_SENSORS.urdf hrp2jsknts_with_3hand.yaml) +endif(${hrp2_models_FOUND}) + # for HIRONXJSK (adding sensor, and fixing some points for gazebo) if(${hrp2_models_FOUND}) attach_sensor_and_endeffector_to_hrp2jsk_urdf(HIRONXJSK HIRONXJSK.urdf diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l index 44183f78..c35e24bf 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l @@ -4,53 +4,126 @@ ) (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 :rhand :joint-list) i) (send robot :joint-list)) (elt av i)) + (setelt av-all (position (elt (send robot :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 :rhand :joint-list) i) (send robot :joint-list)))) + (setelt reference (+ i 6) (elt reference-all (position (elt (send robot :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 _error _sense _h815output _h815motorthermo _pgain _dgain _flimit _controlmode _curlim _fcthreshold _tactiles _thermo) + ) + +(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)) + ((:error __error) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:sense __sense) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:h815output __h815output) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:h815motorthermo __h815motorthermo) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:pgain __pgain) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:dgain __dgain) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:flimit __flimit) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:controlmode __controlmode) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:curlim __curlim) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:fcthreshold __fcthreshold) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:tactiles __tactiles) (make-array 0 :initial-element 0.0 :element-type :float)) + ((:thermo __thermo) (make-array 0 :initial-element 0.0 :element-type :float)) + ) + (setq _reference __reference) + (setq _potentio __potentio) + (setq _error __error) + (setq _sense __sense) + (setq _h815output __h815output) + (setq _h815motorthermo __h815motorthermo) + (setq _pgain __pgain) + (setq _dgain __dgain) + (setq _flimit __flimit) + (setq _controlmode __controlmode) + (setq _curlim __curlim) + (setq _fcthreshold __fcthreshold) + (setq _tactiles __tactiles) + (setq _thermo __thermo) + self) + (:reference + (&optional __reference) + (if __reference (setq _reference __reference)) _reference) + (:potentio + (&optional __potentio) + (if __potentio (setq _potentio __potentio)) _potentio) + (:error + (&optional __error) + (if __error (setq _error __error)) _error) + (:sense + (&optional __sense) + (if __sense (setq _sense __sense)) _sense) + (:h815output + (&optional __h815output) + (if __h815output (setq _h815output __h815output)) _h815output) + (:h815motorthermo + (&optional __h815motorthermo) + (if __h815motorthermo (setq _h815motorthermo __h815motorthermo)) _h815motorthermo) + (:pgain + (&optional __pgain) + (if __pgain (setq _pgain __pgain)) _pgain) + (:dgain + (&optional __dgain) + (if __dgain (setq _dgain __dgain)) _dgain) + (:flimit + (&optional __flimit) + (if __flimit (setq _flimit __flimit)) _flimit) + (:controlmode + (&optional __controlmode) + (if __controlmode (setq _controlmode __controlmode)) _controlmode) + (:curlim + (&optional __curlim) + (if __curlim (setq _curlim __curlim)) _curlim) + (:fcthreshold + (&optional __fcthreshold) + (if __fcthreshold (setq _fcthreshold __fcthreshold)) _fcthreshold) + (:tactiles + (&optional __tactiles) + (if __tactiles (setq _tactiles __tactiles)) _tactiles) + (:thermo + (&optional __thermo) + (if __thermo (setq _thermo __thermo)) _thermo) + ) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l index 1256f3ea..77a050b4 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l @@ -1,10 +1,8 @@ (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l") +(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.l") (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..7c4e84a7 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_with_3hand.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))) @@ -13,7 +11,7 @@ (prog1 (send-super* :init-ending args) (send self :set-additional-end-coords) - (send self :define-min-max-table) + ;;(send self :define-min-max-table) (when (< (norm (send (send (send self :worldcoords) :transformation (send (car (send self :links)) :worldcoords)) :pos)) 0.1) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l index e0cc483c..40aff469 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l @@ -1,10 +1,8 @@ (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l") +(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.l") (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..ec97ff12 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") +(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.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))) @@ -12,7 +10,7 @@ (&rest args) (prog1 (send-super* :init-ending args) - (send self :define-min-max-table) + ;;(send self :define-min-max-table) (when (< (norm (send (send (send self :worldcoords) :transformation (send (car (send self :links)) :worldcoords)) :pos)) 0.1) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l index 53cd1102..4ad8025e 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l @@ -1,120 +1,46 @@ -(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 :rhand args)) + (:larm (send* self :lhand args)) + ((:both :arms) (list (send* self :rhand args) (send* self :lhand args))))) + (:limb + (limb 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 :rhand :angle-vector (subseq av 0 6)) + (send self :lhand :angle-vector (subseq av 6)) ) (concatenate float-vector - (send self :hand :rarm :angle-vector) - (send self :hand :larm :angle-vector)) + (send self :rhand :angle-vector) + (send self :lhand :angle-vector)) ) ) ) - -(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/HRP2JSKNTS_WITH_3HAND.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat new file mode 100644 index 00000000..03969be1 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat @@ -0,0 +1,46 @@ +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 +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_WITH_3HAND.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat new file mode 100644 index 00000000..03969be1 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat @@ -0,0 +1,46 @@ +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 +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_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml index 742b7600..7e167a06 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml @@ -9,6 +9,7 @@ rleg: - RLEG_JOINT3 : rleg-knee-p - RLEG_JOINT4 : rleg-ankle-p - RLEG_JOINT5 : rleg-ankle-r + - RLEG_JOINT6 : rleg-toe-p lleg: - LLEG_JOINT0 : lleg-crotch-y - LLEG_JOINT1 : lleg-crotch-r @@ -16,6 +17,7 @@ lleg: - LLEG_JOINT3 : lleg-knee-p - LLEG_JOINT4 : lleg-ankle-p - LLEG_JOINT5 : lleg-ankle-r + - LLEG_JOINT6 : lleg-toe-p torso: - CHEST_JOINT0 : torso-waist-p - CHEST_JOINT1 : torso-waist-y @@ -30,12 +32,7 @@ rarm: - 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 + - RARM_JOINT7 : rarm-thumb-r larm: - LARM_JOINT0 : larm-shoulder-p - LARM_JOINT1 : larm-shoulder-r @@ -44,39 +41,312 @@ larm: - LARM_JOINT4 : larm-wrist-y - 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-2p + - L_INDEXPIP_R : larm-f1-2r - L_MIDDLEPIP_R : larm-f2-2r ## ## end-coords ## rleg-end-coords: - translate : [0.0, 0, -0.091849] +# Original foot +# translate : [-0.079411, -0.01, -0.031] +# Karasawa sole plate [3mm] + translate : [-0.079411, -0.01, -0.034] lleg-end-coords: - translate : [0.0, 0, -0.091849] +# Original foot +# translate : [-0.079411, 0.01, -0.031] +# Karasawa sole plate [3mm] + translate : [-0.079411, 0.01, -0.034] +torso-end-coords: + translate : [-0.032, 0.0, -1.0557] +head-end-coords: + translate : [0.079, 0.0, 0.1335] + rotate : [0.0, 1.0, 0.0, 99.9999] rarm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] + translate : [-0.0042, 0.0392, -0.1245] + rotate : [0.0, 1.0, 0.0, 90.0] + parent : RARM_LINK6 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] + translate : [-0.0042, -0.0392, -0.1245] + rotate : [0.0, 1.0, 0.0, 90.0] + parent : LARM_LINK6 + +sensors: + - sensor_name: lfsensor + parent_link: LLEG_LINK5 + translate: '0 0 -0.105' + sensor_type: 'base_force6d' + - sensor_name: rfsensor + parent_link: RLEG_LINK5 + translate: '0 0 -0.105' + sensor_type: 'base_force6d' + - sensor_name: lhsensor + parent_link: LARM_LINK6 + translate: '0 0 -0.077' + sensor_type: 'base_force6d' + - sensor_name: rhsensor + parent_link: RARM_LINK6 + translate: '0 0 -0.077' + sensor_type: 'base_force6d' + - sensor_name: CARMINE + parent_link: HEAD_LINK1 + translate: '0.093 0.017 0.131' + rotate: '-0.606685 0.62878 -0.486384 126.205' + sensor_type: 'camera' + # - sensor_name: camera_link + # parent_link: CARMINE + # translate: '-0.045 0.0 0.0' + # rotate: '0.577311 -0.577311 0.577429 120.013' + # sensor_type: 'camera' + # - sensor_name: camera_rgb_frame + # parent_link: camera_link + # translate: '0 -0.045 0' + # sensor_type: 'camera' + # - sensor_name: camera_rgb_optical_frame + # parent_link: camera_rgb_frame + # translate: '0 0 0' + # rotate: '-0.577311 0.577429 -0.577311 120.013' + # sensor_type: 'camera' + # - sensor_name: camera_depth_frame + # parent_link: camera_link + # translate: '0 -0.02 0' + # sensor_type: 'camera' + # - sensor_name: camera_depth_optical_frame + # parent_link: camera_depth_frame + # tranlsate: '0 0 0' + # rotate: '-0.577311 0.577429 -0.577311 120.013' + # sensor_type: 'camera' + - sensor_name: LARM_cb_jig + parent_link: LARM_LINK6 + translate: '0 0.2 -0.2' + rotate: '0.0 0.0 1.0 90' + sensor_type: 'camera' + - sensor_name: RARM_cb_jig + parent_link: RARM_LINK6 + translate: '0 -0.2 -0.2' + rotate: '0.0 0.0 1.0 90' + sensor_type: 'camera' + - sensor_name: fisheye + parent_link: CHEST_LINK1 + translate: '0.13 0.0 0.21' + rotate: '0.92269 -0.000735 0.385543 179.965' + sensor_type: 'camera' ## ## reset-pose ## angle-vector: - reset-pose : [0.0, -1, -20, 40, -20, 1, - 0.0, 1, -20, 40, -20, -1, - 0.0, 0.0, + reset-pose : [0.0, 0, -26, 50, -24, 0, 0, + 0.0, 0, -26, 50, -24, 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, - 20.0, 10.0, 0.0, -30.0, 0.0, 0.0, 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, + 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, + 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: + tag: mass + attribute_name: value + attribute_value: '0' + replaced_xml: '' + - match_rule: + tag: inertia + attribute_name: ixx + attribute_value: '0' + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: LLEG_LINK5 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: RLEG_LINK5 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: LLEG_LINK6 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: RLEG_LINK6 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: limit + attribute_name: velocity + attribute_value: 0.5 + replaced_attribute_value: 10.0 + - match_rule: + tag: limit + attribute_name: effort + attribute_value: 100 + replaced_attribute_value: 1000 + - match_rule: + tag: dynamics + attribute_name: friction + attribute_value: 0 + replaced_attribute_value: 1 + - match_rule: + tag: dynamics + attribute_name: damping + attribute_value: 0.2 + replaced_attribute_value: 1 + # for hands + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXMP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXMP_P_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_MIDDLEPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_THUMBCM_Y_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_THUMBCM_P_LINK + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXMP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXMP_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_MIDDLEPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_THUMBCM_Y_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_THUMBCM_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXMP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXMP_P_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_MIDDLEPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_THUMBCM_Y_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_THUMBCM_P_LINK + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXMP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXMP_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_MIDDLEPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_THUMBCM_Y_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_THUMBCM_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml index 742b7600..bf785271 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml @@ -9,6 +9,7 @@ rleg: - RLEG_JOINT3 : rleg-knee-p - RLEG_JOINT4 : rleg-ankle-p - RLEG_JOINT5 : rleg-ankle-r + - RLEG_JOINT6 : rleg-toe-p lleg: - LLEG_JOINT0 : lleg-crotch-y - LLEG_JOINT1 : lleg-crotch-r @@ -16,9 +17,10 @@ lleg: - LLEG_JOINT3 : lleg-knee-p - LLEG_JOINT4 : lleg-ankle-p - LLEG_JOINT5 : lleg-ankle-r + - LLEG_JOINT6 : lleg-toe-p torso: - - CHEST_JOINT0 : torso-waist-p - - CHEST_JOINT1 : torso-waist-y + - CHEST_JOINT0 : torso-waist-y + - CHEST_JOINT1 : torso-waist-p head: - HEAD_JOINT0 : head-neck-y - HEAD_JOINT1 : head-neck-p @@ -30,12 +32,7 @@ rarm: - 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 + - RARM_JOINT7 : rarm-thumb-r larm: - LARM_JOINT0 : larm-shoulder-p - LARM_JOINT1 : larm-shoulder-r @@ -44,39 +41,318 @@ larm: - LARM_JOINT4 : larm-wrist-y - 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-2p + - L_INDEXPIP_R : larm-f1-2r - L_MIDDLEPIP_R : larm-f2-2r ## ## end-coords ## rleg-end-coords: - translate : [0.0, 0, -0.091849] +# Original foot +# translate : [-0.079411, -0.01, -0.031] +# Karasawa sole plate [3mm] + translate : [-0.079411, -0.01, -0.034] lleg-end-coords: - translate : [0.0, 0, -0.091849] +# Original foot +# translate : [-0.079411, 0.01, -0.031] +# Karasawa sole plate [3mm] + translate : [-0.079411, 0.01, -0.034] +torso-end-coords: + translate : [-0.032, 0.0, -1.0557] +head-end-coords: + translate : [0.079, 0.0, 0.1335] + rotate : [0.0, 1.0, 0.0, 99.9999] rarm-end-coords: - translate : [0, 0, -0.1] - rotate : [0, 1, 0, 90] + translate : [-0.0042, 0.0392, -0.1245] + rotate : [0.0, 1.0, 0.0, 90.0] + parent : RARM_LINK6 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] + translate : [-0.0042, -0.0392, -0.1245] + rotate : [0.0, 1.0, 0.0, 90.0] + parent : LARM_LINK6 + +sensors: + - sensor_name: lfsensor + parent_link: LLEG_LINK5 + translate: '0 0 -0.105' + sensor_type: 'base_force6d' + - sensor_name: rfsensor + parent_link: RLEG_LINK5 + translate: '0 0 -0.105' + sensor_type: 'base_force6d' + - sensor_name: lhsensor + parent_link: LARM_LINK6 + translate: '0 0 -0.077' + sensor_type: 'base_force6d' + - sensor_name: rhsensor + parent_link: RARM_LINK6 + translate: '0 0 -0.077' + sensor_type: 'base_force6d' +# - sensor_name: CARMINE +# parent_link: HEAD_LINK1 +# translate: '0.093 0.017 0.131' +# rotate: '-0.606685 0.62878 -0.486384 126.205' +# sensor_type: 'camera' +# - sensor_name: camera_link +# parent_link: CARMINE +# translate: '-0.045 0.0 0.0' +# rotate: '0.577311 -0.577311 0.577429 120.013' +# sensor_type: 'camera' + - sensor_name: camera_link + parent_link: HEAD_LINK1 + translate: '0.053 0.052 0.119' + rotate: '0.029745 0.997072 -0.070452 17.6947' + sensor_type: 'camera' + - sensor_name: camera_rgb_frame + parent_link: camera_link + translate: '0 -0.045 0' + sensor_type: 'camera' + - sensor_name: camera_rgb_optical_frame + parent_link: camera_rgb_frame + translate: '0 0 0' + rotate: '-0.577311 0.577429 -0.577311 120.013' + sensor_type: 'camera' + - sensor_name: camera_depth_frame + parent_link: camera_link + translate: '0 -0.02 0' + sensor_type: 'camera' + - sensor_name: camera_depth_optical_frame + parent_link: camera_depth_frame + tranlsate: '0 0 0' + rotate: '-0.577311 0.577429 -0.577311 120.013' + sensor_type: 'camera' + - sensor_name: LARM_cb_jig + parent_link: LARM_LINK6 + translate: '0 0.2 -0.2' + rotate: '0.0 0.0 1.0 90' + sensor_type: 'camera' + - sensor_name: RARM_cb_jig + parent_link: RARM_LINK6 + translate: '0 -0.2 -0.2' + rotate: '0.0 0.0 1.0 90' + sensor_type: 'camera' + - sensor_name: fisheye + parent_link: CHEST_LINK1 + translate: '0.13 0.0 0.21' + rotate: '0.92269 -0.000735 0.385543 179.965' + sensor_type: 'camera' + ## ## reset-pose ## angle-vector: - reset-pose : [0.0, -1, -20, 40, -20, 1, - 0.0, 1, -20, 40, -20, -1, - 0.0, 0.0, + reset-pose : [0.0, 0, -26, 50, -24, 0, 0, + 0.0, 0, -26, 50, -24, 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, - 20.0, 10.0, 0.0, -30.0, 0.0, 0.0, 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, + 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, + 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: + tag: mass + attribute_name: value + attribute_value: '0' + replaced_xml: '' + - match_rule: + tag: inertia + attribute_name: ixx + attribute_value: '0' + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: LLEG_LINK5 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: RLEG_LINK5 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: LLEG_LINK6 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: RLEG_LINK6 + replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: limit + attribute_name: velocity + attribute_value: 0.5 + replaced_attribute_value: 10.0 + - match_rule: + tag: limit + attribute_name: effort + attribute_value: 100 + replaced_attribute_value: 1000 + - match_rule: + tag: dynamics + attribute_name: friction + attribute_value: 0 + replaced_attribute_value: 1 + - match_rule: + tag: dynamics + attribute_name: damping + attribute_value: 0.2 + replaced_attribute_value: 1 + # for hands + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXMP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXMP_P_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_INDEXPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_MIDDLEPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_THUMBCM_Y_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: L_THUMBCM_P_LINK + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXMP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXMP_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_INDEXPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_MIDDLEPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_THUMBCM_Y_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: L_THUMBCM_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXMP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXMP_P_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_INDEXPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_MIDDLEPIP_R_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_THUMBCM_Y_LINK + replaced_xml: '' + - match_rule: + tag: inertia + parent_depth: 2 + parent_attribute_name: name + parent_attribute_value: R_THUMBCM_P_LINK + replaced_xml: '' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXMP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXMP_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_INDEXPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_MIDDLEPIP_R_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_THUMBCM_Y_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' + - match_rule: + tag: gazebo + attribute_name: reference + attribute_value: R_THUMBCM_P_LINK + replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' 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": From 4e79c8e1e6ec1707d08ab7e3c131d3da6831889f Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 9 May 2021 03:28:32 +0900 Subject: [PATCH 2/4] [hrpsys_ros_bridge_tutorials/hrp2jsknt*] use :limb :rhand --- .../euslisp/hrp2-common-interface.l | 70 ++----------------- .../euslisp/hrp3hand-utils.l | 16 ++--- .../models/hrp2jsknt_with_3hand.yaml | 1 + .../models/hrp2jsknts_with_3hand.yaml | 1 + 4 files changed, 15 insertions(+), 73 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l index c35e24bf..c50cd0e9 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l @@ -8,8 +8,8 @@ (av &optional (tm 1000)) (let ((av-all (instantiate float-vector (length (send robot :angle-vector))))) (dotimes (i 6) - (setelt av-all (position (elt (send robot :rhand :joint-list) i) (send robot :joint-list)) (elt av i)) - (setelt av-all (position (elt (send robot :lhand :joint-list) i) (send robot :joint-list)) (elt av (+ 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)) @@ -36,8 +36,8 @@ (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 :rhand :joint-list) i) (send robot :joint-list)))) - (setelt reference (+ i 6) (elt reference-all (position (elt (send robot :lhand :joint-list) i) (send robot :joint-list))))) + (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) @@ -48,7 +48,7 @@ (defclass hrp3hand-state :super propertied-object - :slots (_reference _potentio _error _sense _h815output _h815motorthermo _pgain _dgain _flimit _controlmode _curlim _fcthreshold _tactiles _thermo) + :slots (_reference _potentio) ) (defmethod hrp3hand-state @@ -56,33 +56,9 @@ (&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)) - ((:error __error) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:sense __sense) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:h815output __h815output) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:h815motorthermo __h815motorthermo) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:pgain __pgain) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:dgain __dgain) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:flimit __flimit) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:controlmode __controlmode) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:curlim __curlim) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:fcthreshold __fcthreshold) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:tactiles __tactiles) (make-array 0 :initial-element 0.0 :element-type :float)) - ((:thermo __thermo) (make-array 0 :initial-element 0.0 :element-type :float)) ) (setq _reference __reference) (setq _potentio __potentio) - (setq _error __error) - (setq _sense __sense) - (setq _h815output __h815output) - (setq _h815motorthermo __h815motorthermo) - (setq _pgain __pgain) - (setq _dgain __dgain) - (setq _flimit __flimit) - (setq _controlmode __controlmode) - (setq _curlim __curlim) - (setq _fcthreshold __fcthreshold) - (setq _tactiles __tactiles) - (setq _thermo __thermo) self) (:reference (&optional __reference) @@ -90,40 +66,4 @@ (:potentio (&optional __potentio) (if __potentio (setq _potentio __potentio)) _potentio) - (:error - (&optional __error) - (if __error (setq _error __error)) _error) - (:sense - (&optional __sense) - (if __sense (setq _sense __sense)) _sense) - (:h815output - (&optional __h815output) - (if __h815output (setq _h815output __h815output)) _h815output) - (:h815motorthermo - (&optional __h815motorthermo) - (if __h815motorthermo (setq _h815motorthermo __h815motorthermo)) _h815motorthermo) - (:pgain - (&optional __pgain) - (if __pgain (setq _pgain __pgain)) _pgain) - (:dgain - (&optional __dgain) - (if __dgain (setq _dgain __dgain)) _dgain) - (:flimit - (&optional __flimit) - (if __flimit (setq _flimit __flimit)) _flimit) - (:controlmode - (&optional __controlmode) - (if __controlmode (setq _controlmode __controlmode)) _controlmode) - (:curlim - (&optional __curlim) - (if __curlim (setq _curlim __curlim)) _curlim) - (:fcthreshold - (&optional __fcthreshold) - (if __fcthreshold (setq _fcthreshold __fcthreshold)) _fcthreshold) - (:tactiles - (&optional __tactiles) - (if __tactiles (setq _tactiles __tactiles)) _tactiles) - (:thermo - (&optional __thermo) - (if __thermo (setq _thermo __thermo)) _thermo) ) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l index 4ad8025e..539a72a8 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l @@ -2,11 +2,11 @@ '( (:hand (arm &rest args) (case arm - (:rarm (send* self :rhand args)) - (:larm (send* self :lhand args)) - ((:both :arms) (list (send* self :rhand args) (send* self :lhand args))))) + (: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 method &rest args) + (limb &optional method &rest args) (if (member limb '(:rhand :lhand)) (let ((av (case method (:open-pose (float-vector 0 60 -10 30 0 0)) @@ -35,12 +35,12 @@ (:hand-angle-vector (&optional (av)) (when av - (send self :rhand :angle-vector (subseq av 0 6)) - (send self :lhand :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 :rhand :angle-vector) - (send self :lhand :angle-vector)) + (send self :limb :rhand :angle-vector) + (send self :limb :lhand :angle-vector)) ) ) ) diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml index 7e167a06..39b76645 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.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 diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml index bf785271..06695a2a 100644 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.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 From 3a09f7efd8de05f2492f0e77e1a4eefae51b62c7 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Thu, 3 Jun 2021 19:15:08 +0900 Subject: [PATCH 3/4] [hrpsys_ros_bridge_tutorials] overwrite HRP2JSKNT* by HRP2JSKNT*_WITH_3HAND --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 128 ++----- .../euslisp/hrp2jsknt-interface.l | 2 +- .../euslisp/hrp2jsknt-utils.l | 4 +- .../euslisp/hrp2jsknts-interface.l | 2 +- .../euslisp/hrp2jsknts-utils.l | 4 +- .../euslisp/hrp3hand-utils.l | 6 +- .../models/HRP2JSKNT.PDgain.dat | 14 +- .../models/HRP2JSKNT.urdf.xacro | 4 +- .../models/HRP2JSKNTS.PDgain.dat | 14 +- .../models/HRP2JSKNTS.urdf.xacro | 11 - .../models/HRP2JSKNTS_WITH_3HAND.PDgain.dat | 46 --- .../models/HRP2JSKNT_WITH_3HAND.PDgain.dat | 46 --- .../models/hrp2jsknt.yaml | 27 +- .../models/hrp2jsknt_with_3hand.yaml | 353 ----------------- .../models/hrp2jsknts.yaml | 25 +- .../models/hrp2jsknts_with_3hand.yaml | 359 ------------------ 16 files changed, 112 insertions(+), 933 deletions(-) delete mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.urdf.xacro delete mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat delete mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat delete mode 100644 hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml delete mode 100644 hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index cbac2721..6953127f 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -151,7 +151,7 @@ compile_openhrp_model_for_closed_robots(HRP2JSK HRP2JSK_for_OpenHRP3 HRP2JSK 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 +160,25 @@ 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 "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.PDgain.dat" --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --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" + --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" --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(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 +187,24 @@ 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 "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.PDgain.dat" --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --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" + --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" --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(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" @@ -246,74 +248,6 @@ compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G ) gen_minmax_table_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G) -compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT_WITH_3HAND - --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," - --conf-file-option "# Original foot + 7dof joint version" - --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,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 "# 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, 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,100,100,100,100,100,100,100,100,100,100,100,100" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT_WITH_3HAND.PDgain.dat" - --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --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" - --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" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --conf-dt-option "0.004" - --simulation-timestep-option "0.004" - -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 - ) -#gen_minmax_table_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenHRP3 HRP2JSKNT_WITH_3HAND) - -compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS_WITH_3HAND - --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," - --conf-file-option "# Original foot + 7dof joint version" - --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,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 "# 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, 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,100,100,100,100,100,100,100,100,100,100,100,100" - --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat" - --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --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" - --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" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" - --conf-dt-option "0.004" - --simulation-timestep-option "0.004" - -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 - ) -#gen_minmax_table_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_OpenHRP3 HRP2JSKNTS_WITH_3HAND) - 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 @@ -538,8 +472,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") @@ -662,27 +596,23 @@ 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 HRP2JSKNT_WITH_3HAND, HRP2JSKNTS_WITH_3HAND (adding sensor, and fixing some points for gazebo) +# for HRP2JSKNTS if(${hrp2_models_FOUND}) - attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNT HRP2JSKNT_WITH_3HAND.urdf - HRP2JSKNT_WITH_3HAND_SENSORS.urdf hrp2jsknt_with_3hand.yaml) - attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND.urdf - HRP2JSKNTS_WITH_3HAND_SENSORS.urdf hrp2jsknts_with_3hand.yaml) + 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) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l index 77a050b4..1b59ab27 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-interface.l @@ -1,5 +1,5 @@ (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.l") +(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l") (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") diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l index 7c4e84a7..0081fcf8 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknt-utils.l @@ -1,4 +1,4 @@ -(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.l") +(require :hrp2jsknt "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknt.l") (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l") (eval @@ -11,7 +11,7 @@ (prog1 (send-super* :init-ending args) (send self :set-additional-end-coords) - ;;(send self :define-min-max-table) + (send self :define-min-max-table) (when (< (norm (send (send (send self :worldcoords) :transformation (send (car (send self :links)) :worldcoords)) :pos)) 0.1) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l index 40aff469..91dde7a9 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l @@ -1,5 +1,5 @@ (load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.l") +(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l") (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") diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l index ec97ff12..dd3ed308 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l @@ -1,4 +1,4 @@ -(load "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.l") +(require :hrp2jsknts "package://hrpsys_ros_bridge_tutorials/models/hrp2jsknts.l") (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l") (eval @@ -10,7 +10,7 @@ (&rest args) (prog1 (send-super* :init-ending args) - ;;(send self :define-min-max-table) + (send self :define-min-max-table) (when (< (norm (send (send (send self :worldcoords) :transformation (send (car (send self :links)) :worldcoords)) :pos)) 0.1) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l index 539a72a8..f5e25728 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hrp3hand-utils.l @@ -42,5 +42,9 @@ (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)) + ) ) diff --git a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat index 6fbf55ef..03969be1 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat @@ -31,4 +31,16 @@ 1400 100 460 100 1870 100 -0 0 \ No newline at end of file +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..03969be1 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat @@ -31,4 +31,16 @@ 1400 100 460 100 1870 100 -0 0 \ No newline at end of file +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/HRP2JSKNTS_WITH_3HAND.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat deleted file mode 100644 index 03969be1..00000000 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS_WITH_3HAND.PDgain.dat +++ /dev/null @@ -1,46 +0,0 @@ -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 -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_WITH_3HAND.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat deleted file mode 100644 index 03969be1..00000000 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT_WITH_3HAND.PDgain.dat +++ /dev/null @@ -1,46 +0,0 @@ -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 -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.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 39b76645..00000000 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknt_with_3hand.yaml +++ /dev/null @@ -1,353 +0,0 @@ -## -## - collada_joint_name : euslisp_joint_name (start with :) -## -limbs: [rleg, lleg, torso, head, rarm, larm, rhand, lhand] - -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 - - RLEG_JOINT6 : rleg-toe-p -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 - - LLEG_JOINT6 : lleg-toe-p -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 - - RARM_JOINT7 : rarm-thumb-r -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 - - 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 -## -rleg-end-coords: -# Original foot -# translate : [-0.079411, -0.01, -0.031] -# Karasawa sole plate [3mm] - translate : [-0.079411, -0.01, -0.034] -lleg-end-coords: -# Original foot -# translate : [-0.079411, 0.01, -0.031] -# Karasawa sole plate [3mm] - translate : [-0.079411, 0.01, -0.034] -torso-end-coords: - translate : [-0.032, 0.0, -1.0557] -head-end-coords: - translate : [0.079, 0.0, 0.1335] - rotate : [0.0, 1.0, 0.0, 99.9999] -rarm-end-coords: - translate : [-0.0042, 0.0392, -0.1245] - rotate : [0.0, 1.0, 0.0, 90.0] - parent : RARM_LINK6 -larm-end-coords: - translate : [-0.0042, -0.0392, -0.1245] - rotate : [0.0, 1.0, 0.0, 90.0] - parent : LARM_LINK6 - -sensors: - - sensor_name: lfsensor - parent_link: LLEG_LINK5 - translate: '0 0 -0.105' - sensor_type: 'base_force6d' - - sensor_name: rfsensor - parent_link: RLEG_LINK5 - translate: '0 0 -0.105' - sensor_type: 'base_force6d' - - sensor_name: lhsensor - parent_link: LARM_LINK6 - translate: '0 0 -0.077' - sensor_type: 'base_force6d' - - sensor_name: rhsensor - parent_link: RARM_LINK6 - translate: '0 0 -0.077' - sensor_type: 'base_force6d' - - sensor_name: CARMINE - parent_link: HEAD_LINK1 - translate: '0.093 0.017 0.131' - rotate: '-0.606685 0.62878 -0.486384 126.205' - sensor_type: 'camera' - # - sensor_name: camera_link - # parent_link: CARMINE - # translate: '-0.045 0.0 0.0' - # rotate: '0.577311 -0.577311 0.577429 120.013' - # sensor_type: 'camera' - # - sensor_name: camera_rgb_frame - # parent_link: camera_link - # translate: '0 -0.045 0' - # sensor_type: 'camera' - # - sensor_name: camera_rgb_optical_frame - # parent_link: camera_rgb_frame - # translate: '0 0 0' - # rotate: '-0.577311 0.577429 -0.577311 120.013' - # sensor_type: 'camera' - # - sensor_name: camera_depth_frame - # parent_link: camera_link - # translate: '0 -0.02 0' - # sensor_type: 'camera' - # - sensor_name: camera_depth_optical_frame - # parent_link: camera_depth_frame - # tranlsate: '0 0 0' - # rotate: '-0.577311 0.577429 -0.577311 120.013' - # sensor_type: 'camera' - - sensor_name: LARM_cb_jig - parent_link: LARM_LINK6 - translate: '0 0.2 -0.2' - rotate: '0.0 0.0 1.0 90' - sensor_type: 'camera' - - sensor_name: RARM_cb_jig - parent_link: RARM_LINK6 - translate: '0 -0.2 -0.2' - rotate: '0.0 0.0 1.0 90' - sensor_type: 'camera' - - sensor_name: fisheye - parent_link: CHEST_LINK1 - translate: '0.13 0.0 0.21' - rotate: '0.92269 -0.000735 0.385543 179.965' - sensor_type: 'camera' - -## -## reset-pose -## -angle-vector: - reset-pose : [0.0, 0, -26, 50, -24, 0, 0, - 0.0, 0, -26, 50, -24, 0, 0, - 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, - 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, - 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: - tag: mass - attribute_name: value - attribute_value: '0' - replaced_xml: '' - - match_rule: - tag: inertia - attribute_name: ixx - attribute_value: '0' - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: LLEG_LINK5 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: RLEG_LINK5 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: LLEG_LINK6 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: RLEG_LINK6 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: limit - attribute_name: velocity - attribute_value: 0.5 - replaced_attribute_value: 10.0 - - match_rule: - tag: limit - attribute_name: effort - attribute_value: 100 - replaced_attribute_value: 1000 - - match_rule: - tag: dynamics - attribute_name: friction - attribute_value: 0 - replaced_attribute_value: 1 - - match_rule: - tag: dynamics - attribute_name: damping - attribute_value: 0.2 - replaced_attribute_value: 1 - # for hands - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXMP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXMP_P_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_MIDDLEPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_THUMBCM_Y_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_THUMBCM_P_LINK - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXMP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXMP_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_MIDDLEPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_THUMBCM_Y_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_THUMBCM_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXMP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXMP_P_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_MIDDLEPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_THUMBCM_Y_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_THUMBCM_P_LINK - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXMP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXMP_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_MIDDLEPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_THUMBCM_Y_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_THUMBCM_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' 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 06695a2a..00000000 --- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts_with_3hand.yaml +++ /dev/null @@ -1,359 +0,0 @@ -## -## - collada_joint_name : euslisp_joint_name (start with :) -## -limbs: [rleg, lleg, torso, head, rarm, larm, rhand, lhand] - -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 - - RLEG_JOINT6 : rleg-toe-p -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 - - LLEG_JOINT6 : lleg-toe-p -torso: - - CHEST_JOINT0 : torso-waist-y - - CHEST_JOINT1 : torso-waist-p -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 - - RARM_JOINT7 : rarm-thumb-r -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 - - 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 -## -rleg-end-coords: -# Original foot -# translate : [-0.079411, -0.01, -0.031] -# Karasawa sole plate [3mm] - translate : [-0.079411, -0.01, -0.034] -lleg-end-coords: -# Original foot -# translate : [-0.079411, 0.01, -0.031] -# Karasawa sole plate [3mm] - translate : [-0.079411, 0.01, -0.034] -torso-end-coords: - translate : [-0.032, 0.0, -1.0557] -head-end-coords: - translate : [0.079, 0.0, 0.1335] - rotate : [0.0, 1.0, 0.0, 99.9999] -rarm-end-coords: - translate : [-0.0042, 0.0392, -0.1245] - rotate : [0.0, 1.0, 0.0, 90.0] - parent : RARM_LINK6 -larm-end-coords: - translate : [-0.0042, -0.0392, -0.1245] - rotate : [0.0, 1.0, 0.0, 90.0] - parent : LARM_LINK6 - -sensors: - - sensor_name: lfsensor - parent_link: LLEG_LINK5 - translate: '0 0 -0.105' - sensor_type: 'base_force6d' - - sensor_name: rfsensor - parent_link: RLEG_LINK5 - translate: '0 0 -0.105' - sensor_type: 'base_force6d' - - sensor_name: lhsensor - parent_link: LARM_LINK6 - translate: '0 0 -0.077' - sensor_type: 'base_force6d' - - sensor_name: rhsensor - parent_link: RARM_LINK6 - translate: '0 0 -0.077' - sensor_type: 'base_force6d' -# - sensor_name: CARMINE -# parent_link: HEAD_LINK1 -# translate: '0.093 0.017 0.131' -# rotate: '-0.606685 0.62878 -0.486384 126.205' -# sensor_type: 'camera' -# - sensor_name: camera_link -# parent_link: CARMINE -# translate: '-0.045 0.0 0.0' -# rotate: '0.577311 -0.577311 0.577429 120.013' -# sensor_type: 'camera' - - sensor_name: camera_link - parent_link: HEAD_LINK1 - translate: '0.053 0.052 0.119' - rotate: '0.029745 0.997072 -0.070452 17.6947' - sensor_type: 'camera' - - sensor_name: camera_rgb_frame - parent_link: camera_link - translate: '0 -0.045 0' - sensor_type: 'camera' - - sensor_name: camera_rgb_optical_frame - parent_link: camera_rgb_frame - translate: '0 0 0' - rotate: '-0.577311 0.577429 -0.577311 120.013' - sensor_type: 'camera' - - sensor_name: camera_depth_frame - parent_link: camera_link - translate: '0 -0.02 0' - sensor_type: 'camera' - - sensor_name: camera_depth_optical_frame - parent_link: camera_depth_frame - tranlsate: '0 0 0' - rotate: '-0.577311 0.577429 -0.577311 120.013' - sensor_type: 'camera' - - sensor_name: LARM_cb_jig - parent_link: LARM_LINK6 - translate: '0 0.2 -0.2' - rotate: '0.0 0.0 1.0 90' - sensor_type: 'camera' - - sensor_name: RARM_cb_jig - parent_link: RARM_LINK6 - translate: '0 -0.2 -0.2' - rotate: '0.0 0.0 1.0 90' - sensor_type: 'camera' - - sensor_name: fisheye - parent_link: CHEST_LINK1 - translate: '0.13 0.0 0.21' - rotate: '0.92269 -0.000735 0.385543 179.965' - sensor_type: 'camera' - - -## -## reset-pose -## -angle-vector: - reset-pose : [0.0, 0, -26, 50, -24, 0, 0, - 0.0, 0, -26, 50, -24, 0, 0, - 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, - 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, - 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: - tag: mass - attribute_name: value - attribute_value: '0' - replaced_xml: '' - - match_rule: - tag: inertia - attribute_name: ixx - attribute_value: '0' - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: LLEG_LINK5 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: RLEG_LINK5 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: LLEG_LINK6 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: RLEG_LINK6 - replaced_xml: '\n 1400000.0\n 280.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: limit - attribute_name: velocity - attribute_value: 0.5 - replaced_attribute_value: 10.0 - - match_rule: - tag: limit - attribute_name: effort - attribute_value: 100 - replaced_attribute_value: 1000 - - match_rule: - tag: dynamics - attribute_name: friction - attribute_value: 0 - replaced_attribute_value: 1 - - match_rule: - tag: dynamics - attribute_name: damping - attribute_value: 0.2 - replaced_attribute_value: 1 - # for hands - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXMP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXMP_P_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_INDEXPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_MIDDLEPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_THUMBCM_Y_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: L_THUMBCM_P_LINK - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXMP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXMP_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_INDEXPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_MIDDLEPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_THUMBCM_Y_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: L_THUMBCM_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXMP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXMP_P_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_INDEXPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_MIDDLEPIP_R_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_THUMBCM_Y_LINK - replaced_xml: '' - - match_rule: - tag: inertia - parent_depth: 2 - parent_attribute_name: name - parent_attribute_value: R_THUMBCM_P_LINK - replaced_xml: '' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXMP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXMP_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_INDEXPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_MIDDLEPIP_R_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_THUMBCM_Y_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' - - match_rule: - tag: gazebo - attribute_name: reference - attribute_value: R_THUMBCM_P_LINK - replaced_xml: '\n 140.0\n 2800.0\n 1.5\n 1.5\n 1 0 0\n 10.0\n 0.00\n ' From 74f41ba71200f1ead7a37943083b3e2a8928ef6c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Wed, 23 Jun 2021 14:51:14 +0900 Subject: [PATCH 4/4] [hrpsys_ros_bridge_tutorials/HRP2*] use same gains (real and sim) --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 23 ++--- .../models/HRP2G.PDgain.dat | 23 +++++ .../models/HRP2G.PDgains_sim.dat | 23 +++++ .../models/HRP2G_withoutGrippers.PDgain.dat | 23 +++++ .../models/HRP2JSK.PDgain.dat | 69 +++++++------ .../models/HRP2JSK.PDgains_sim.dat | 37 +++++++ .../models/HRP2JSKNT.PDgain.dat | 99 ++++++++++--------- .../models/HRP2JSKNT.PDgains_sim.dat | 53 ++++++++++ .../models/HRP2JSKNTS.PDgain.dat | 99 ++++++++++--------- .../models/HRP2JSKNTS.PDgains_sim.dat | 53 ++++++++++ .../models/HRP2W.PDgain.dat | 23 +++++ .../models/HRP2W.PDgains_sim.dat | 23 +++++ .../models/HRP2W_withoutGrippers.PDgain.dat | 23 +++++ 13 files changed, 434 insertions(+), 137 deletions(-) create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2G.PDgain.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2G.PDgains_sim.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2G_withoutGrippers.PDgain.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSK.PDgains_sim.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgains_sim.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgains_sim.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2W.PDgain.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2W.PDgains_sim.dat create mode 100644 hrpsys_ros_bridge_tutorials/models/HRP2W_withoutGrippers.PDgain.dat diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 6953127f..0fd1734e 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -139,12 +139,11 @@ 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" ) @@ -168,12 +167,11 @@ compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND_for_OpenH --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,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 "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" --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" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --conf-dt-option "0.004" --simulation-timestep-option "0.004" ) @@ -195,12 +193,11 @@ compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND_for_Ope --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,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 "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" --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" - --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" --conf-dt-option "0.004" --simulation-timestep-option "0.004" ) @@ -219,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" @@ -240,8 +237,8 @@ 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" 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 03969be1..4b941e35 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNT.PDgain.dat @@ -1,46 +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 -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 +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/HRP2JSKNTS.PDgain.dat b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat index 03969be1..4b941e35 100644 --- a/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat +++ b/hrpsys_ros_bridge_tutorials/models/HRP2JSKNTS.PDgain.dat @@ -1,46 +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 -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 +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/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