diff --git a/.gitignore b/.gitignore
index 05d20775..4f4ed89b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -49,4 +49,13 @@ hrpsys_ros_bridge_tutorials/scripts/*_unstable_hrpsys_config.py
# generated files in hrpsys_tutorials
hrpsys_tutorials/models/*.conf
-hrpsys_tutorials/models/*.xml
\ No newline at end of file
+hrpsys_tutorials/models/*.xml
+
+# generated files in hrpsys_gazebo_tutorials
+hrpsys_gazebo_tutorials/config/HRP2JSKNTS.conf
+hrpsys_gazebo_tutorials/config/rtcdRobotModeHRP2Common.conf
+hrpsys_gazebo_tutorials/launch/hrp2017_auditor_gazebo.launch
+hrpsys_gazebo_tutorials/launch/hrp2017_gazebo.launch
+hrpsys_gazebo_tutorials/launch/hrp2017_ros_bridge_gazebo.launch
+hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf
+hrpsys_gazebo_tutorials/scripts/jsk_hrp2_gazebo_setup.py
diff --git a/hrpsys_gazebo_tutorials/CMakeLists.txt b/hrpsys_gazebo_tutorials/CMakeLists.txt
index f3c7b7e8..7a520420 100644
--- a/hrpsys_gazebo_tutorials/CMakeLists.txt
+++ b/hrpsys_gazebo_tutorials/CMakeLists.txt
@@ -1,7 +1,9 @@
cmake_minimum_required(VERSION 2.8.3)
project(hrpsys_gazebo_tutorials)
-find_package(catkin REQUIRED COMPONENTS euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials hrpsys_gazebo_general)
+find_package(catkin REQUIRED COMPONENTS euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials hrpsys_gazebo_general jsk_hrp2_ros_bridge hrp2_models euscollada xacro)
+
+catkin_python_setup()
set(PKG_CONFIG_PATH ${hrpsys_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH})
find_package(PkgConfig)
@@ -10,6 +12,8 @@ pkg_check_modules(hrpsys hrpsys-base REQUIRED)
pkg_check_modules(collada_urdf_jsk_patch collada_urdf_jsk_patch)
catkin_package(CATKIN_DEPENDS euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials hrpsys_gazebo_general)
+
+add_subdirectory(rtc)
catkin_add_env_hooks(99.hrpsys_gazebo_tutorials SHELLS bash zsh
DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
@@ -18,3 +22,34 @@ catkin_add_env_hooks(99.hrpsys_gazebo_tutorials SHELLS bash zsh
install(DIRECTORY euslisp worlds launch config environment_models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN ".svn" EXCLUDE)
install(DIRECTORY scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+
+if (jsk_hrp2_ros_bridge_SOURCE_PREFIX)
+ set(jsk_hrp2_ros_bridge_PACKAGE_PATH ${jsk_hrp2_ros_bridge_SOURCE_PREFIX})
+endif()
+if (hrp2_models_SOURCE_PREFIX)
+ set(hrp2_models_PACKAGE_PATH ${hrp2_models_SOURCE_PREFIX})
+endif()
+if (hrpsys_gazebo_general_SOURCE_PREFIX)
+ set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_SOURCE_PREFIX})
+endif()
+
+execute_process(COMMAND sed -e "s#@MODULE_LOAD_PATH@#${PROJECT_SOURCE_DIR}/lib,${hrpsys_gazebo_general_PACKAGE_PATH}/lib,${hrpsys_PREFIX}/lib#g" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/config/rtcdRobotModeHRP2Common.conf.in"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/config/rtcdRobotModeHRP2Common.conf)
+
+execute_process(COMMAND sed -e "s#^model:.*$#model: ${hrp2_models_PACKAGE_PATH}/HRP2JSKNTS_for_OpenHRP3/HRP2JSKNTSmain.wrl#g" -e "s#^pdgains.file_name:.*$#pdgains.file_name: ${jsk_hrp2_ros_bridge_PACKAGE_PATH}/iob/PDgainsHRP2JSKNTS.sav#g" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/config/HRP2JSKNTS.conf"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/config/HRP2JSKNTS.conf)
+
+execute_process(COMMAND sed -e "s#/home/grxuser/prog/rtmros_hrp2/jsk_hrp2_ros_bridge/force_sensor_calib_data/hand-calib_HRP2JSKNTS_[0-9]\\+#${PROJECT_SOURCE_DIR}/config/hand-calib_HRP2JSKNTS#g" -e "s#/home/grxuser/prog/rtmros_hrp2/jsk_hrp2_ros_bridge#${PROJECT_SOURCE_DIR}#g" -e "s#/home/grxuser/prog/rtmros_hrp2/hrp2_models#${hrp2_models_PACKAGE_PATH}#g" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/scripts/jsk_hrp2_setup.py"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/scripts/jsk_hrp2_gazebo_setup.py)
+
+execute_process(COMMAND "${PROJECT_SOURCE_DIR}/scripts/replace_xmls.py" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/launch/hrp2017.launch" -C "${PROJECT_SOURCE_DIR}/config/hrp2017_gazebo.yaml"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/launch/hrp2017_gazebo.launch)
+
+execute_process(COMMAND "${PROJECT_SOURCE_DIR}/scripts/replace_xmls.py" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/launch/hrp2017_ros_bridge.launch" -C "${PROJECT_SOURCE_DIR}/config/hrp2017_ros_bridge_gazebo.yaml"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/launch/hrp2017_ros_bridge_gazebo.launch)
+
+execute_process(COMMAND "${PROJECT_SOURCE_DIR}/scripts/replace_xmls.py" "${jsk_hrp2_ros_bridge_PACKAGE_PATH}/launch/hrp2017_auditor.launch" -C "${PROJECT_SOURCE_DIR}/config/hrp2017_auditor_gazebo.yaml"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/launch/hrp2017_auditor_gazebo.launch)
+
+execute_process(COMMAND "${xacro_PREFIX}/lib/xacro/xacro" "${PROJECT_SOURCE_DIR}/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro"
+ OUTPUT_FILE ${PROJECT_SOURCE_DIR}/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf)
diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml
index 232e7e44..b8cf8e70 100644
--- a/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml
+++ b/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml
@@ -1,7 +1,9 @@
hrpsys_gazebo_configuration:
### velocity feedback for joint control, use parameter gains/joint_name/p_v
- use_velocity_feedback: true
- use_joint_effort: true
+ use_velocity_feedback: false
+ use_joint_effort: false
+ use_pd_feedback: true
+ use_servo_on: true
iob_rate: 250
### loose synchronization default true
# use_loose_synchronized: false
@@ -11,7 +13,7 @@ hrpsys_gazebo_configuration:
### name of robot (using for namespace)
robotname: HRP2JSKNTS
### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id
- joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
+ joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33]
### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints)
joints:
- RLEG_JOINT0
@@ -48,64 +50,42 @@ hrpsys_gazebo_configuration:
- LARM_JOINT5
- LARM_JOINT6
- LARM_JOINT7
- - L_THUMBCM_Y
- - L_THUMBCM_P
- - L_INDEXMP_R
- - L_INDEXMP_P
- - L_INDEXPIP_R
- - L_MIDDLEPIP_R
- - R_THUMBCM_Y
- - R_THUMBCM_P
- - R_INDEXMP_R
- - R_INDEXMP_P
- - R_INDEXPIP_R
- - R_MIDDLEPIP_R
## jointid:
gains:
- CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0}
- CHEST_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0}
- HEAD_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- HEAD_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0}
- LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # CRTOCH-Y
- LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-R
- LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0} # CRTOCH-P
- LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0} # KNEE-P
- LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-P
- LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0} # ANKLE-R
- LLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0} # TOE-R
- RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0}
- RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
- RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0}
- RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0}
- RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
- RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
- RLEG_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0}
- L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
- R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0}
+ CHEST_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0}
+ CHEST_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0}
+ HEAD_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ HEAD_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ LARM_JOINT7: {p: 0.0, d: 0.0, i: 0.0, i_clamp: 0.0, p_v: 0.0, vp: 0.0}
+ RARM_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT6: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 150.0, vp: 0.0}
+ RARM_JOINT7: {p: 0.0, d: 0.0, i: 0.0, i_clamp: 0.0, p_v: 0.0, vp: 0.0}
+ LLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0} # CRTOCH-Y
+ LLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0} # CRTOCH-R
+ LLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0, vp: 0.0} # CRTOCH-P
+ LLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0, vp: 0.0} # KNEE-P
+ LLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0} # ANKLE-P
+ LLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0} # ANKLE-R
+ LLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0} # TOE-R
+ RLEG_JOINT0: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0}
+ RLEG_JOINT1: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0}
+ RLEG_JOINT2: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 325.0, vp: 0.0}
+ RLEG_JOINT3: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 450.0, vp: 0.0}
+ RLEG_JOINT4: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0}
+ RLEG_JOINT5: {p: 700.0, d: 0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0, vp: 0.0}
+ RLEG_JOINT6: {p: 120.0, d: 0.013, i: 0.0, i_clamp: 0.0, p_v: 250.0, vp: 0.0}
force_torque_sensors:
- rfsensor
diff --git a/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml b/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml
new file mode 100644
index 00000000..c4af9ce6
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/HRP3HAND.yaml
@@ -0,0 +1,44 @@
+hrp3hand_hrpsys_gazebo_configuration:
+### velocity feedback for joint control, use parameter gains/joint_name/p_v
+ use_velocity_feedback: false
+ use_joint_effort: false
+ use_pd_feedback: true
+ use_servo_on: true
+ iob_rate: 250
+### loose synchronization default true
+ use_loose_synchronized: false
+### synchronized hrpsys and gazebo
+# use_synchronized_command: false
+# iob_substeps: 5
+### name of robot (using for namespace)
+ robotname: HRP3HAND
+### joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id
+ joint_id_list: [ 6, 7, 8, 9, 10, 11, 0, 1, 2, 3, 4, 5]
+### joints : list used in gazebo, sizeof(joint_id_list) == sizeof(joints)
+ joints:
+ - L_THUMBCM_Y
+ - L_THUMBCM_P
+ - L_INDEXMP_R
+ - L_INDEXMP_P
+ - L_INDEXPIP_R
+ - L_MIDDLEPIP_R
+ - R_THUMBCM_Y
+ - R_THUMBCM_P
+ - R_INDEXMP_R
+ - R_INDEXMP_P
+ - R_INDEXPIP_R
+ - R_MIDDLEPIP_R
+## jointid:
+ gains:
+ L_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ L_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ L_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ L_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ L_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ L_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_INDEXMP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_INDEXMP_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_INDEXPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_MIDDLEPIP_R: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_THUMBCM_Y: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
+ R_THUMBCM_P: {p: 10.0, d: 0.01, i: 0.0, i_clamp: 0.0, p_v: 10.0, vp: 0.0}
diff --git a/hrpsys_gazebo_tutorials/config/hand-calib_HRP2JSKNTS b/hrpsys_gazebo_tutorials/config/hand-calib_HRP2JSKNTS
new file mode 100644
index 00000000..438f0dc9
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/hand-calib_HRP2JSKNTS
@@ -0,0 +1,4 @@
+lfsensor 0 0 0 0 0 0 0 0 0 0
+lhsensor 0 0 0 0 0 0 0.00538113 -0.00209447 -0.0248345 1.6618102
+rfsensor 0 0 0 0 0 0 0 0 0 0
+rhsensor 0 0 0 0 0 0 0.00538113 0.00209447 -0.0248345 1.6618102
diff --git a/hrpsys_gazebo_tutorials/config/hrp2017_auditor_gazebo.yaml b/hrpsys_gazebo_tutorials/config/hrp2017_auditor_gazebo.yaml
new file mode 100644
index 00000000..cd5d3f7c
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/hrp2017_auditor_gazebo.yaml
@@ -0,0 +1,7 @@
+replace_xmls:
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_pcl_ros)/launch/openni2_remote.launch'
+ replaced_xml: ' '
diff --git a/hrpsys_gazebo_tutorials/config/hrp2017_gazebo.yaml b/hrpsys_gazebo_tutorials/config/hrp2017_gazebo.yaml
new file mode 100644
index 00000000..2bf08cdb
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/hrp2017_gazebo.yaml
@@ -0,0 +1,55 @@
+replace_xmls:
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/hrp2017_ros_bridge.launch'
+ replaced_attribute_name: file
+ replaced_attribute_value: '$(find hrpsys_gazebo_tutorials)/launch/hrp2017_ros_bridge_gazebo.launch'
+ - match_rule:
+ tag: arg
+ attribute:
+ - name: name
+ value: NTP_HOSTNAME
+ parent_tag: include
+ parent_attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/jsk_robot_common.launch'
+ replaced_attribute_name: value
+ replaced_attribute_value: localhost
+ - match_rule:
+ tag: node
+ attribute:
+ - name: name
+ value: thermo_publisher
+ replaced_xml: ''
+ - match_rule:
+ tag: node
+ attribute:
+ - name: name
+ value: thermo_publisher2
+ replaced_xml: ''
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_pcl_ros)/launch/openni2_local.launch'
+ replaced_xml: ''
+ - match_rule:
+ tag: node
+ attribute:
+ - name: pkg
+ value: dynamic_reconfigure
+ replaced_xml: ''
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/chest_camera.launch'
+ replaced_xml: ''
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find voice_text)/launch/voice_text.launch'
+ replaced_xml: ''
diff --git a/hrpsys_gazebo_tutorials/config/hrp2017_ros_bridge_gazebo.yaml b/hrpsys_gazebo_tutorials/config/hrp2017_ros_bridge_gazebo.yaml
new file mode 100644
index 00000000..da04430d
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/hrp2017_ros_bridge_gazebo.yaml
@@ -0,0 +1,24 @@
+replace_xmls:
+ - match_rule:
+ tag: arg
+ attribute:
+ - name: name
+ value: nameserver
+ parent_tag: include
+ parent_attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/jsk_hrp2_ros_bridge.launch'
+ replaced_attribute_name: default
+ replaced_attribute_value: localhost
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/jsk_hrp2_ros_bridge.launch'
+ added_xml: ''
+ - match_rule:
+ tag: include
+ attribute:
+ - name: file
+ value: '$(find jsk_hrp2_ros_bridge)/launch/jsk_hrp2_ros_bridge.launch'
+ added_xml: ''
diff --git a/hrpsys_gazebo_tutorials/config/hrpsys_dashboard.yaml b/hrpsys_gazebo_tutorials/config/hrpsys_dashboard.yaml
new file mode 100644
index 00000000..de95b273
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/config/hrpsys_dashboard.yaml
@@ -0,0 +1,5 @@
+hrpsys_rtcd_start_command: "xterm -T \"RTCD\" -e 'PS1=user && source ~/.$(basename $SHELL)rc && rossetlocal && `rospack find hrpsys_gazebo_tutorials`/scripts/rtcd_start_command.sh --remote'"
+hrpsys_rtcd_stop_command: "xterm -e \"pkill openhrp-model-l* && pkill rtcd\""
+
+hrpsys_ros_bridge_start_command: "xterm -T \"Auditor\" -e \"PS1=user && source ~/.$(basename $SHELL)rc && rossetlocal && `rospack find hrpsys_gazebo_tutorials`/scripts/hrp2-launch-ros-bridge.sh\""
+hrpsys_ros_bridge_stop_command: "xterm -e \"source ~/.$(basename $SHELL)rc && `rospack find hrpsys_gazebo_tutorials`/scripts/kill-ros-bridge.sh\""
diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch
index 119e6074..b26faff8 100644
--- a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch
+++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers.launch
@@ -3,11 +3,12 @@
+
+ file="$(find jsk_hrp2_ros_bridge)/config/HRP2JSKNTS_gazebo_private.yaml" ns="HRP2JSKNTS" />
+ file="$(find hrpsys_gazebo_tutorials)/config/HRP3HAND.yaml" ns="HRP3HAND" />
@@ -19,5 +20,8 @@
+
+
+
diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_room73b2.launch b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_room73b2.launch
new file mode 100644
index 00000000..4331b270
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/launch/gazebo_hrp2jsknts_no_controllers_room73b2.launch
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hrpsys_gazebo_tutorials/launch/hrp2_dashboard_gazebo.launch b/hrpsys_gazebo_tutorials/launch/hrp2_dashboard_gazebo.launch
new file mode 100644
index 00000000..69de9c24
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/launch/hrp2_dashboard_gazebo.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
diff --git a/hrpsys_gazebo_tutorials/launch/modelloader.launch b/hrpsys_gazebo_tutorials/launch/modelloader.launch
new file mode 100644
index 00000000..661293b2
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/launch/modelloader.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hrpsys_gazebo_tutorials/launch/rtcd.launch b/hrpsys_gazebo_tutorials/launch/rtcd.launch
new file mode 100644
index 00000000..b5086403
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/launch/rtcd.launch
@@ -0,0 +1,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hrpsys_gazebo_tutorials/package.xml b/hrpsys_gazebo_tutorials/package.xml
index 7c2303c8..4bae03e4 100644
--- a/hrpsys_gazebo_tutorials/package.xml
+++ b/hrpsys_gazebo_tutorials/package.xml
@@ -14,6 +14,9 @@
openhrp3
hrpsys
euscollada
+ jsk_hrp2_ros_bridge
+ hrp2_models
+ xacro
openhrp3
hrpsys
@@ -22,5 +25,11 @@
hrpsys_ros_bridge_tutorials
hrpsys_gazebo_general
collada_urdf_jsk_patch
+ jsk_hrp2_ros_bridge
+ hrp2_models
+
+
+
+
diff --git a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro
index 327cd1af..241fd883 100644
--- a/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro
+++ b/hrpsys_gazebo_tutorials/robot_models/HRP2JSKNTS/HRP2JSKNTS.urdf.xacro
@@ -7,6 +7,12 @@
hrpsys_gazebo_configuration
+
+
+ HRP3HAND
+ hrp3hand_hrpsys_gazebo_configuration
+
+
@@ -45,43 +51,43 @@
1
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
20
@@ -99,12 +105,12 @@
true
20
- xtion/rgb/image_raw
- xtion/rgb/camera_info
- xtion/depth/image_raw
- xtion/depth/camera_info
- xtion/depth/points
- xtion_depth_optical_frame
+ camera_remote/rgb/image_raw
+ camera_remote/rgb/camera_info
+ camera_remote/depth/image_raw
+ camera_remote/depth/camera_info
+ camera_remote/depth/points
+ camera_depth_optical_frame
0.0
0.0
0.0
@@ -145,4 +151,47 @@
BODY
+
+
+
+ HRP2JSKNTS
+ CHEST_LINK1
+ 1.2
+ 0.5
+ 0.1
+ 0.03
+ 2500
+ 500
+ 10000
+
+
+
+
+ HRP2JSKNTS
+ BODY
+ ChestCranePlugin
+ 1.0
+ 0.4
+ 0.1
+ 0.03
+ 2500
+ 500
+ 10000
+ false
+
+
+
+
+
+ HRP2JSKNTS
+ RLEG_thermo
+
+
+
+
+ HRP2JSKNTS
+ LLEG_thermo
+
+
+
diff --git a/hrpsys_gazebo_tutorials/rqt_plugin.xml b/hrpsys_gazebo_tutorials/rqt_plugin.xml
new file mode 100644
index 00000000..f4b71648
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/rqt_plugin.xml
@@ -0,0 +1,22 @@
+
+
+
+ A Python GUI plugin for displaying a dashboard that displays and interacts with low level hrpsys states.
+
+
+
+
+ folder
+ Plugins related to specific robots.
+
+
+
+ folder
+ Plugins related to the hrpsys robot.
+
+
+ task-past-due
+ A Python GUI plugin for displaying a dashboard that displays and interacts with low level hrpsys states.
+
+
+
diff --git a/hrpsys_gazebo_tutorials/rtc/CMakeLists.txt b/hrpsys_gazebo_tutorials/rtc/CMakeLists.txt
new file mode 100644
index 00000000..9180b004
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/rtc/CMakeLists.txt
@@ -0,0 +1,15 @@
+find_package(PkgConfig)
+pkg_check_modules(omniorb omniORB4 REQUIRED)
+pkg_check_modules(omnidynamic omniDynamic4 REQUIRED)
+pkg_check_modules(openrtm_aist openrtm-aist REQUIRED)
+pkg_check_modules(openhrp3 openhrp3.1 REQUIRED)
+pkg_check_modules(hrpsys hrpsys-base REQUIRED)
+
+list(GET hrpsys_INCLUDE_DIRS 0 hrpsys_first_incdir)
+list(APPEND hrpsys_INCLUDE_DIRS ${hrpsys_first_incdir}/hrpsys)
+list(APPEND hrpsys_INCLUDE_DIRS ${hrpsys_first_incdir}/hrpsys/idl)
+
+include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS} ${hrpsys_INCLUDE_DIRS})
+link_directories(${CATKIN_DEVEL_PREFIX}/lib ${hrpsys_PREFIX}/lib ${openhrp3_LIBRARY_DIRS} /opt/ros/$ENV{ROS_DISTRO}/lib/)
+
+add_subdirectory(HRP3HandController)
diff --git a/hrpsys_gazebo_tutorials/rtc/HRP3HandController/CMakeLists.txt b/hrpsys_gazebo_tutorials/rtc/HRP3HandController/CMakeLists.txt
new file mode 100644
index 00000000..c6f6920c
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/rtc/HRP3HandController/CMakeLists.txt
@@ -0,0 +1,58 @@
+if(EXISTS ${hrpsys_SOURCE_DIR})
+ set(SequencePlayerDirectory ${hrpsys_SOURCE_DIR}/src/rtc/SequencePlayer)
+elseif(EXISTS ${hrpsys_SOURCE_PREFIX})
+ set(SequencePlayerDirectory ${hrpsys_SOURCE_PREFIX}/src/rtc/SequencePlayer)
+else()
+ set(SequencePlayerDirectory ${hrpsys_PREFIX}/share/hrpsys/src/rtc/SequencePlayer)
+endif()
+
+if(EXISTS ${hrpsys_gazebo_general_SOURCE_DIR})
+ set(iobDirectory ${hrpsys_gazebo_general_SOURCE_DIR}/iob)
+else(EXISTS ${hrpsys_gazebo_general_SOURCE_PREFIX})
+ set(iobDirectory ${hrpsys_gazebo_general_SOURCE_PREFIX}/iob)
+endif()
+
+find_package(jsk_hrp2_ros_bridge)
+list(GET jsk_hrp2_ros_bridge_INCLUDE_DIRS 0 jsk_hrp2_ros_bridge_first_incdir)
+list(APPEND jsk_hrp2_ros_bridge_INCLUDE_DIRS ${jsk_hrp2_ros_bridge_first_incdir}/jsk_hrp2_ros_bridge)
+list(APPEND jsk_hrp2_ros_bridge_INCLUDE_DIRS ${jsk_hrp2_ros_bridge_first_incdir}/jsk_hrp2_ros_bridge/idl)
+
+set(HRP3HANDCONTROLLER_SOURCE ${jsk_hrp2_ros_bridge_SOURCE_PREFIX}/rtc/HRP3HandController)
+
+set(comp_sources handcontrol.cpp ${HRP3HANDCONTROLLER_SOURCE}/HRP3HandController.cpp ${HRP3HANDCONTROLLER_SOURCE}/HRP3HandControllerService_impl.cpp ${SequencePlayerDirectory}/interpolator.cpp)
+set(libs hrpIo2_gazebo hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub HRP3HandControllerServiceStub)
+
+if ("$ENV{HRP2NO}" STREQUAL "16")
+ set(HRP2NO "-DHRPNO=16")
+elseif ("$ENV{HRP2NO}" STREQUAL "17")
+ set(HRP2NO "-DHRPNO=17")
+else ()
+ set(HRP2NO "-DHRPNO=16")
+endif()
+
+set(handcontrol_compile_flags "${HRP2NO}")
+
+include_directories(${jsk_hrp2_ros_bridge_INCLUDE_DIRS} ${HRP3HANDCONTROLLER_SOURCE} ${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${SequencePlayerDirectory} ${iobDirectory})
+link_directories(${jsk_hrp2_ros_bridge_PREFIX}/lib)
+
+add_definitions(-std=c++14)
+
+message ("compile HRP3HandController")
+add_library(HRP3HandController SHARED ${comp_sources})
+target_link_libraries(HRP3HandController ${libs})
+set_target_properties(HRP3HandController PROPERTIES PREFIX "")
+set_target_properties(HRP3HandController PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+set_target_properties(HRP3HandController PROPERTIES COMPILE_FLAGS ${handcontrol_compile_flags})
+
+add_executable(HRP3HandControllerComp ${HRP3HANDCONTROLLER_SOURCE}/HRP3HandControllerComp.cpp ${comp_sources})
+target_link_libraries(HRP3HandControllerComp ${libs} ${omniorb_LIBRARIES} ${omnidynamic_LIBRARIES} RTC coil dl pthread ${catkin_LIBRARIES})
+set_target_properties(HRP3HandControllerComp PROPERTIES COMPILE_FLAGS ${handcontrol_compile_flags})
+
+install(TARGETS HRP3HandController
+ LIBRARY DESTINATION lib CONFIGURATIONS Release Debug RELWITHDEBINFO
+ )
+install(TARGETS HRP3HandController HRP3HandControllerComp
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/lib
+ )
diff --git a/hrpsys_gazebo_tutorials/rtc/HRP3HandController/handcontrol.cpp b/hrpsys_gazebo_tutorials/rtc/HRP3HandController/handcontrol.cpp
new file mode 100644
index 00000000..7ce25c67
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/rtc/HRP3HandController/handcontrol.cpp
@@ -0,0 +1,660 @@
+/*
+ largely modified by Naoki-Hiraoka
+
+ from
+ File : handcontrol.cpp
+ Date : Fri Apr 28 12:36:15 JST 2006
+ Author : Kei Okada
+*/
+
+#include
+#include
+#include
+
+#include "handcontrol.h"
+
+#include "iob2.h"
+static iob hc_iob;
+
+static UsbRobotState usb_state;
+static double set_angle[DOF];
+static double default_flimit[MOTOR_NO];
+#define pGAIN -5.0
+#define dGAIN 0.0
+
+handcontrolPlugin_impl::handcontrolPlugin_impl() {
+}
+
+bool handcontrolPlugin_impl::setup(robot_state *rs,motor_command *mc) {
+ const char idstr[] = "$Id: handcontrol.cpp $";
+ fprintf(stderr, "---- handcontrolPlugin::setup ----\n");
+ fprintf(stderr, "---- > %s, dt = %f[s]\n", idstr, m_dt);
+ angle = new interpolator(DOF, m_dt, interpolator::HOFFARBIB);
+
+ hc_iob.set_number_of_joints(DOF);
+ hc_iob.set_number_of_force_sensors(0);
+ hc_iob.set_number_of_gyro_sensors(0);
+ hc_iob.set_number_of_accelerometers(0);
+
+ hc_iob.set_signal_period(long(1e9 * m_dt));
+ hc_iob.open_iob("HANDCONTROL");
+
+ setServoInfo();
+ for (int i = 0; i < DOF; i++) output[i] = 0;
+ angle->set(output);
+
+ ControlModeVector(usb_state.Controlmode);
+ GetRobotState(0);
+ for (int i = 0; i < DOF; i++) {
+ output[i] = usb_state.Potentio[i];
+ }
+ if ( debug_level > 1 ) {
+ for (int i = 0; i < DOF; i++) {
+ fprintf(stderr, "%7.3f ", output[i]);
+ }
+ fprintf(stderr,"\n");
+ }
+ angle->set(output);
+ is_body_servoOff = true;
+
+ return true;
+}
+
+void handcontrolPlugin_impl::control(robot_state *rs,motor_command *mc) {
+ struct timeval tv0,tv1;
+ static int count = 0;
+ if ( (count++) % 200 == 0 ) {
+ if ( is_body_servoOff && usb_state.Controlmode[0] != 8) { // servo off
+ double modes[DOF];
+ for (int i = 0 ; i < MOTOR_NO; i++ ) modes[i] = 8 ;
+ SetControlModeVector(modes, NULL);
+ fprintf(stderr, ";; [handcontrol] move to zero output mode.. \n");
+ }
+ }
+
+ if ( debug_level > 2 ) {
+ fprintf(stderr, "get start ...\n");
+ gettimeofday(&tv0,NULL);
+ }
+ GetRobotState(0);
+ if ( debug_level > 2 ) {
+ gettimeofday(&tv1,NULL);
+ fprintf(stderr, "get done ... %7.3f [msec]\n",
+ 1000 * (double) tv1.tv_sec + (double) tv1.tv_usec / 1000
+ - (1000 * (double) tv0.tv_sec + (double) tv0.tv_usec / 1000)
+ );
+ }
+
+ if ( !angle->isEmpty () ) {
+ angle->get(output);
+
+ if ( debug_level > 2 ) {
+ for (int i = 0; i < DOF; i++) {
+ fprintf(stderr, "%7.3f ", output[i]);
+ }
+ fprintf(stderr,"\n");
+ }
+ }
+ // debug
+ SetAngleVector(output);
+}
+
+bool handcontrolPlugin_impl::cleanup(robot_state *rs,motor_command *mc) {
+ delete angle;
+ double modes[DOF];
+ for (int i = 0 ; i < MOTOR_NO; i++ ) modes[i] = 8 ;
+ SetControlModeVector(modes, NULL);
+ SetAngleVector(output);
+ hc_iob.close_iob();
+ usleep(1000);
+
+ usleep(1000);
+ return true;
+}
+
+void handcontrolPlugin_impl::m_joint_angles(istringstream& strm) {
+ double tmp_angle[DOF], a = 0.0, tm = 1.0;
+
+ for ( int i = 0; i < DOF; i++){
+ if ( strm >> a ) {
+ tmp_angle[i] = a;
+ } else {
+ fprintf(stderr, "Invalid length of angle vector. Too short!!\n");
+ return;
+ }
+ }
+ if (!(strm >> tm)){
+ fprintf(stderr, "Invalid length of angle vector. Too short!!\n");
+ return;
+ }
+ if (strm >> a){
+ fprintf(stderr, "Invalid length of angle vector. Too long!!\n");
+ return;
+ }
+ setJointAngles(tmp_angle, tm);
+}
+
+void handcontrolPlugin_impl::m_wait_interpolation(istringstream& strm) {
+ waitInterpolation();
+}
+
+void handcontrolPlugin_impl::waitInterpolation()
+{
+ while ( !angle->isEmpty() ) {
+ usleep(5000);
+ }
+ usleep(5000);
+};
+
+bool handcontrolPlugin_impl::setJointAngles(const double* jvs, const double tm)
+{
+ angle->setGoal(jvs,tm);
+
+ if ( debug_level > 1 ) {
+ cerr << "emptyp = " << angle->isEmpty() << endl;
+ cout << "tmp_angle : ";
+ for (int i = 0; i < DOF; i++){
+ cerr << jvs[i];
+ }
+ cerr << ", tm = " << tm << endl;
+ }
+};
+
+void handcontrolPlugin_impl::m_set_interpolation_method(istringstream& strm) {
+ string str;
+ strm >> str;
+
+ interpolator::interpolation_mode imode;
+ if ( str == ":linear" ) {
+ imode = interpolator::LINEAR;
+ cerr << "Using interpolator::LINEAR mode" << endl;
+ }else{
+ imode = interpolator::HOFFARBIB;
+ cerr << "Using interpolator::HOFFARBIB mode" << endl;
+ }
+ while ( ! angle->isEmpty() ) {
+ usleep(5000);
+ }
+ angle = new interpolator(DOF,m_dt, imode);
+ angle->set(output);
+}
+
+void handcontrolPlugin_impl::m_pgain_vector(istringstream& strm) {
+ double tmp_gain[DOF], a = pGAIN;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ tmp_gain[i] = a;
+ }
+ SetPGainVector(tmp_gain);
+ fprintf(stderr, "pgain : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f", tmp_gain[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::m_dgain_vector(istringstream& strm) {
+ double tmp_gain[DOF], a = dGAIN;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ tmp_gain[i] = a;
+ }
+ SetDGainVector(tmp_gain);
+ fprintf(stderr, "dgain : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f\n", tmp_gain[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::m_flimit_vector(istringstream& strm) {
+ double tmp_limit[DOF], a = 20;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ tmp_limit[i] = a;
+ }
+ SetFLimitVector(tmp_limit);
+ fprintf(stderr, "flimit : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f\n", tmp_limit[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::m_fcthreshold_vector(istringstream& strm) {
+ double tmp_thre[DOF], a = 3;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ tmp_thre[i] = a;
+ }
+ SetFCThresholdVector(tmp_thre);
+ fprintf(stderr, "fcthreshold : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f\n", tmp_thre[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::m_controlmode_vector(istringstream& strm) {
+ double tmp_mode[DOF], a = 8;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ tmp_mode[i] = a;
+ }
+ SetControlModeVector(tmp_mode, output);
+ fprintf(stderr, "controlmode : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f\n", tmp_mode[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::m_flimit_gain_vector(istringstream& strm) {
+ double tmp_limit[DOF], a = 20;
+ for ( int i = 0; i < DOF; i++){
+ strm >> a;
+ if ( !((1.0 >= a) && (a >= 0.0)) ) {
+ fprintf(stderr, "hand flimit gain out of range\n");
+ return;
+ }
+ tmp_limit[i] = a * default_flimit[i];
+ }
+ SetFLimitVector(tmp_limit);
+ fprintf(stderr, "flimit : ");
+ for ( int i = 0; i < DOF; i++) fprintf(stderr, "%7.3f\n", tmp_limit[i]);
+ fprintf(stderr, "\n");
+}
+
+void handcontrolPlugin_impl::getRobotState(dsequence_out reference,
+ dsequence_out potentio,
+ dsequence_out error,
+ dsequence_out sense,
+ dsequence_out houtput,
+ dsequence_out motorthermo,
+ dsequence_out pgain,
+ dsequence_out dgain,
+ dsequence_out flimit,
+ dsequence_out controlmode,
+ dsequence_out curlim,
+ dsequence_out fcthreshold,
+ dsequence_out tactiles,
+ dsequence_out thermo
+ ) {
+ reference = new dsequence;
+ potentio = new dsequence;
+ error = new dsequence;
+ sense = new dsequence;
+ houtput = new dsequence;
+ motorthermo = new dsequence;
+ pgain = new dsequence;
+ dgain = new dsequence;
+ flimit = new dsequence;
+ controlmode = new dsequence;
+ curlim = new dsequence;
+ fcthreshold = new dsequence;
+ tactiles = new dsequence;
+ thermo = new dsequence;
+
+ reference->length(MOTOR_NO);
+ potentio->length(MOTOR_NO);
+ error->length(MOTOR_NO);
+ sense->length(MOTOR_NO);
+ houtput->length(MOTOR_NO);
+ motorthermo->length(MOTOR_NO);
+ pgain->length(MOTOR_NO);
+ dgain->length(MOTOR_NO);
+ flimit->length(MOTOR_NO);
+ controlmode->length(MOTOR_NO);
+ curlim->length(MOTOR_NO);
+ fcthreshold->length(MOTOR_NO);
+ tactiles->length(32);
+ thermo->length(16);
+ for (int i=0; i < 32; i++) tactiles->get_buffer()[i] = 0;
+ for (int i=0; i < 16; i++) thermo->get_buffer()[i] = 0;
+ //
+ for (int i=0; i < MOTOR_NO; i++) reference->get_buffer()[i] = usb_state.Reference[i];
+ for (int i=0; i < MOTOR_NO; i++) potentio->get_buffer()[i] = usb_state.Potentio[i];
+ for (int i=0; i < MOTOR_NO; i++) error->get_buffer()[i] = usb_state.Error[i];
+ for (int i=0; i < MOTOR_NO; i++) sense->get_buffer()[i] = usb_state.Sense[i];
+ for (int i=0; i < MOTOR_NO; i++) houtput->get_buffer()[i] = usb_state.houtput[i];
+ for (int i=0; i < MOTOR_NO; i++) motorthermo->get_buffer()[i] = usb_state.motorthermo[i];
+ for (int i=0; i < MOTOR_NO; i++) pgain->get_buffer()[i] = usb_state.Pgain[i];
+ for (int i=0; i < MOTOR_NO; i++) dgain->get_buffer()[i] = usb_state.Dgain[i];
+ for (int i=0; i < MOTOR_NO; i++) flimit->get_buffer()[i] = usb_state.Flimit[i];
+ for (int i=0; i < MOTOR_NO; i++) controlmode->get_buffer()[i] = usb_state.Controlmode[i];
+ for (int i=0; i < MOTOR_NO; i++) curlim->get_buffer()[i] = usb_state.Curlim[i];
+ for (int i=0; i < MOTOR_NO; i++) fcthreshold->get_buffer()[i] = usb_state.Fcthreshold[i];
+ for (int i=0; i < 16; i++) tactiles->get_buffer()[i] = usb_state.Tactiles[i];
+ for (int i=0; i < 16; i++) thermo->get_buffer()[i] = usb_state.Thermo[i];
+ return;
+}
+
+void handcontrolPlugin_impl::handServoOn()
+{
+ SetControlModeVector(8.0, output);
+ usleep(static_cast(5000));
+ setJointAngles(usb_state.Potentio, 0.5);
+ usleep(static_cast(5000));
+ waitInterpolation();
+ SetControlModeVector(11.0, output);
+ usleep(static_cast(1000*1000));
+};
+
+void handcontrolPlugin_impl::handServoOff()
+{
+ SetControlModeVector(8.0, output);
+ usleep(static_cast(5000));
+};
+
+void handcontrolPlugin_impl::handJointCalib()
+{
+ double calib_pose1[DOF] = {30, 122, 0, 90, 0, -155,
+ 30, 122, 0, 90, 0, -155};
+ double calib_pose2[DOF] = {107, 122, 107, 90, -155, -155,
+ 107, 122, 107, 90, -155, -155};
+ double calib_pose3[DOF] = {0, 0, -95, -95, 40, 40,
+ 0, 0, -95, -95, 40, 40};
+ double calib_pose4[DOF] = {-92, -95, 0, 0, 0, 0,
+ -92, -95, 0, 0, 0, 0};
+ waitInterpolation();
+ for (size_t i = 0; i < DOF; i++) calib_pose1[i] = calib_pose1[i] + usb_state.Potentio[i];
+ //setJointAngles(calib_pose1, 4.0);
+ setJointAngles(calib_pose1, 0.5);
+ waitInterpolation();
+ for (size_t i = 0; i < DOF; i++) calib_pose2[i] = calib_pose2[i] + usb_state.Potentio[i];
+ //setJointAngles(calib_pose2, 4.0);
+ setJointAngles(calib_pose2, 0.5);
+ waitInterpolation();
+ for (size_t i = 0; i < DOF; i++) calib_pose3[i] = calib_pose3[i] + usb_state.Potentio[i];
+ //setJointAngles(calib_pose3, 4.0);
+ setJointAngles(calib_pose3, 0.5);
+ waitInterpolation();
+ for (size_t i = 0; i < DOF; i++) calib_pose4[i] = calib_pose4[i] + usb_state.Potentio[i];
+ //setJointAngles(calib_pose4, 4.0);
+ setJointAngles(calib_pose4, 0.5);
+ waitInterpolation();
+ SetControlModeVector(4, output);
+ usleep(static_cast(1000*1000));
+ SetControlModeVector(11, output);
+ usleep(static_cast(1000*1000));
+ waitInterpolation();
+ double tmpoutput[DOF];
+ for (int i = 0; i < DOF; i++) {
+ tmpoutput[i] = usb_state.Potentio[i];
+ }
+ angle->set(tmpoutput);
+};
+
+#include
+void handcontrolPlugin_impl::handReconnect()
+{
+ fprintf(stderr, "[hc] Reconnect called!!\n");
+}
+
+/*
+ * END copy from ROBOTusbconf_H (hrp2handusbconf.h)
+ */
+
+static struct servoInfo Servo[MOTOR_NO];
+void setServoInfo(void)
+{
+ int i;
+ for (i = 0 ; i < MOTOR_NO; i++ ) {
+ Servo[i].neutralAngle = 0;
+ Servo[i].neutralPulse = 0;
+ Servo[i].neutralPotentio = 0;
+ Servo[i].pulsePerPotentio = 1.0;
+ Servo[i].pulsePerError = 1.0;
+ Servo[i].pgain = -5.0;
+ Servo[i].dgain = 0.0;
+ Servo[i].fc_cycle = 1.0;
+ Servo[i].fc_threshold = 3;
+ Servo[i].f_limit = 20;
+ Servo[i].curlim = 1.0; /* [A] */
+ Servo[i].controlmode = 8; // zeroout
+ Servo[i].usepgain = Servo[i].usedgain = 1;
+ Servo[i].usefccycle = Servo[i].usefcthreshold = Servo[i].useflimit = 1;
+ Servo[i].usecurlim = 1;
+ Servo[i].usecontrolmode = 1;
+
+ hc_iob.write_pgain(i,5.0);
+ hc_iob.write_servo(i,0);
+ hc_iob.write_power_command(i,0);
+ }
+
+#if HRPNO==16
+#warning Compiling handcontrol for hrp2016c
+ default_flimit[RHAND_T_1Y ] = 10.0; // 0
+ default_flimit[RHAND_T_1P ] = 20.0; // 1
+ default_flimit[RHAND_F1_1R] = 10.0; // 2
+ default_flimit[RHAND_F1_1P] = 15.0; // 3
+ default_flimit[RHAND_F1_2R] = 7.5; // 4
+ default_flimit[RHAND_F2_2R] = 20.0*0.95; // 5
+ default_flimit[LHAND_T_1Y ] = 10.0; // 0
+ default_flimit[LHAND_T_1P ] = 20.0; // 1
+ default_flimit[LHAND_F1_1R] = 10.0; // 2
+ default_flimit[LHAND_F1_1P] = 15.0; // 3
+ default_flimit[LHAND_F1_2R] = 7.5; // 4
+ default_flimit[LHAND_F2_2R] = 15.0*0.5; // 5
+#elif HRPNO==17
+#warning Compiling handcontrol for hrp2017c
+ default_flimit[RHAND_T_1Y ] = 10.0; // 0
+ default_flimit[RHAND_T_1P ] = 20.0; // 1
+ default_flimit[RHAND_F1_1R] = 10.0; // 2
+ default_flimit[RHAND_F1_1P] = 15.0; // 3
+ default_flimit[RHAND_F1_2R] = 10.0; // 4
+ default_flimit[RHAND_F2_2R] = 20.0; // 5
+ default_flimit[LHAND_T_1Y ] = 10.0; // 0
+ default_flimit[LHAND_T_1P ] = 20.0; // 1
+ default_flimit[LHAND_F1_1R] = 7.5; // 2
+ default_flimit[LHAND_F1_1P] = 15.0; // 3
+ default_flimit[LHAND_F1_2R] = 7.5; // 4
+ default_flimit[LHAND_F2_2R] = 15.0; // 5
+#else
+#error unsupported devices
+#endif
+ for (int ii = 0; ii < MOTOR_NO; ii++)
+ Servo[ii].f_limit = default_flimit[ii];
+}
+
+
+double* PGainVector(double v[DOF]) {
+ int i;
+ if (v==NULL) return v;
+ for (i=0; iPgain){
+ for(i=0; iPgain[i] = v[i];
+ }}
+ DGainVector(v);
+ if(usb_state->Dgain){
+ for(i=0; iDgain[i] = v[i];
+ }}
+ double ForceRef[MOTOR_NO];
+ FCThresholdVector(v);
+ if(usb_state->Fcthreshold){
+ for(i=0; iFcthreshold[i] = ForceRef[i];
+ }}
+ FLimitVector(v);
+ if(usb_state->Flimit){
+ for(i=0; iFlimit[i] = v[i];
+ }}
+ CurLimVector(v);
+ if(usb_state->Curlim){
+ for(i=0; iCurlim[i] = v[i];
+ }}
+ ControlModeVector(v);
+ if(usb_state->Controlmode){
+ for(i=0; iControlmode[i] = v[i];
+ }}
+}
+
+void Data_to_UsbRobotState(UsbRobotState *usb_state)
+{
+ int i, index;
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ usb_state->Reference[i] = set_angle[i];
+ }
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ double ret;
+ hc_iob.read_actual_angle(i,&ret);
+ usb_state->Potentio[i] = ret * 180 / 3.14 ;
+ }
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ usb_state->Error[i] = usb_state->Reference[i]-usb_state->Potentio[i];
+ }
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ usb_state->Sense[i] = 0.0;
+ }
+
+ for (i = 0; i < 16; i++)
+ {
+ usb_state->Thermo[i] = 0.0;
+ }
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ usb_state->motorthermo[i] = 0.0;
+ }
+
+ for (i = 0; i < MOTOR_NO; i++)
+ {
+ usb_state->houtput[i] = 0.0;
+ }
+
+ for (i = 0; i < 16; i++)
+ {
+ usb_state->Tactiles[i] = 0.0;
+ }
+}
+
+void GetRobotState(int wait_msec)
+{
+ Data_to_UsbRobotState(&usb_state);
+ Set_SendData_to_RobotState(&usb_state);
+ return;
+}
+
+void SetAngleVector(double v[DOF])
+{
+ int i;
+ for (i = 0; i < MOTOR_NO; i++){
+ set_angle[i] = v[i];
+ }
+ double vrad[DOF];
+ for (int i=0;i
+*/
+
+#ifndef JSK_HANDCONTROL_H
+#define JSK_HANDCONTROL_H
+
+#include "interpolator.h"
+#include
+#include
+#include
+
+struct OpenHRP2_RobotState;
+typedef OpenHRP2_RobotState robot_state;
+typedef OpenHRP2_RobotState motor_command;
+#include "HRP3HandControllerService.hh"
+typedef OpenHRP::HRP3HandControllerService::dSequence dsequence;
+typedef OpenHRP::HRP3HandControllerService::dSequence_out dsequence_out;
+
+#include "unistd.h"
+
+#define MOTOR_NO 12
+#define DOF MOTOR_NO
+#define Potentio_num MOTOR_NO
+
+class handcontrolPlugin_impl {
+public:
+ handcontrolPlugin_impl();
+ bool setup(robot_state *rs,motor_command *mc);
+ void control(robot_state *rs,motor_command *mc);
+ bool cleanup(robot_state *rs,motor_command *mc);
+ ;;
+ void m_joint_angles(istringstream& strm);
+ void m_wait_interpolation(istringstream& strm);
+ void m_set_interpolation_method(istringstream& strm);
+ void m_pgain_vector(istringstream& strm);
+ void m_dgain_vector(istringstream& strm);
+ void m_flimit_vector(istringstream& strm);
+ void m_fcthreshold_vector(istringstream& strm);
+ void m_controlmode_vector(istringstream& strm);
+ void m_flimit_gain_vector(istringstream& strm);
+ void set_body_servoOff (const bool _is_servoOff)
+ {
+ is_body_servoOff = _is_servoOff;
+ };
+ void set_debug_level(const size_t _debuglevel)
+ {
+ debug_level = _debuglevel;
+ };
+
+ void getRobotState(dsequence_out reference,
+ dsequence_out potentio,
+ dsequence_out error,
+ dsequence_out sense,
+ dsequence_out houtput,
+ dsequence_out motorthermo,
+ dsequence_out pgain,
+ dsequence_out dgain,
+ dsequence_out flimit,
+ dsequence_out controlmode,
+ dsequence_out curlim,
+ dsequence_out fcthreshold,
+ dsequence_out tactiles,
+ dsequence_out thermo
+ );
+ void handServoOn();
+ void handServoOff();
+ void handJointCalib();
+ void handReconnect();
+ void waitInterpolation();
+ bool setJointAngles(const double* jvs, const double tm);
+ void set_dt (const double _dt) { m_dt = _dt; };
+
+private:
+ interpolator *angle;
+ double output[MOTOR_NO], m_dt;
+ bool is_body_servoOff;
+ size_t debug_level;
+};
+
+#ifndef HRPNO
+#define HRPNO 16
+#endif
+
+#define PGAIN_NO MOTOR_NO
+#define DGAIN_NO MOTOR_NO
+
+#define F_LIMIT_NO MOTOR_NO
+#define CURLIM_NO MOTOR_NO
+#define CONTROL_MODE_NO MOTOR_NO
+#define FC_THRESHOLD_NO MOTOR_NO
+
+#define RHAND_T_1P 1
+#define RHAND_F1_1P 3
+#define RHAND_F1_2R 4
+#define RHAND_T_1Y 0
+#define RHAND_F1_1R 2
+#define RHAND_F2_2R 5
+#define LHAND_T_1P 7
+#define LHAND_F1_1P 9
+#define LHAND_F1_2R 10
+#define LHAND_T_1Y 6
+#define LHAND_F1_1R 8
+#define LHAND_F2_2R 11
+
+#include
+#if HRPNO==16
+#warning Compiling handcontrol for hrp2016c
+#elif HRPNO==17
+#warning Compiling handcontrol for hrp2017c
+#endif
+
+struct servoInfo {
+ double neutralAngle;
+ double neutralPulse;
+ double neutralPotentio;
+ double pulsePerAngle;
+ double pulsePerPotentio;
+ double pulsePerError;
+ double pgain, dgain;
+ double fc_cycle, fc_threshold;
+ double f_limit;
+ double curlim;
+ int controlmode;
+ unsigned usepgain:1, usedgain:1;
+ unsigned usefccycle:1, usefcthreshold:1, useflimit:1;
+ unsigned usecurlim:1;
+ unsigned usecontrolmode:1;
+};
+void setServoInfo(void);/**/
+
+void GetRobotState(int wait_msec);/**/
+void SetAngleVector(double v[DOF]);/**/
+double* ControlModeVector(double modes[DOF]);/**/
+double* SetPGainVector(double v[DOF]);/**/
+double* SetDGainVector(double v[DOF]);/**/
+double* SetFLimitVector(double v[DOF]);/**/
+double* SetFCThresholdVector(double v[DOF]);/**/
+double* SetControlModeVector(double modes[DOF], double output[DOF]);/**/
+double* SetControlModeVector(double mode, double output[DOF]);/**/
+
+typedef struct {
+ double Reference[MOTOR_NO];
+ double Potentio[MOTOR_NO];
+ double Error[MOTOR_NO];
+ double Sense[MOTOR_NO];
+ double Tactiles[16];
+ double Thermo[16];
+ double houtput[MOTOR_NO];
+ double motorthermo[MOTOR_NO];
+ double Pgain[MOTOR_NO];/**/
+ double Dgain[MOTOR_NO];/**/
+ double Flimit[MOTOR_NO];/**/
+ double Controlmode[MOTOR_NO];/**/
+ double Curlim[MOTOR_NO];/**/
+ double Fcthreshold[MOTOR_NO];/**/
+} UsbRobotState;
+
+#endif /* JSK_HANDCONTROL_H */
diff --git a/hrpsys_gazebo_tutorials/scripts/hrp2-launch-ros-bridge.sh b/hrpsys_gazebo_tutorials/scripts/hrp2-launch-ros-bridge.sh
new file mode 100755
index 00000000..26a08f98
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/hrp2-launch-ros-bridge.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+FNAME1=/tmp/rosbridge_${USER}_`date +%Y-%m-%d_%H%M%S`.log
+FNAME2=/tmp/auditor_${USER}_`date +%Y-%m-%d_%H%M%S`.log
+roslaunch hrpsys_gazebo_tutorials hrp`expr $HRP2NO + 2000`_gazebo.launch 2>&1 | tee ${FNAME1} & roslaunch hrpsys_gazebo_tutorials hrp`expr $HRP2NO + 2000`_auditor_gazebo.launch 2>&1 | tee ${FNAME2}
diff --git a/hrpsys_gazebo_tutorials/scripts/hrp2017_gazebo_setup.py b/hrpsys_gazebo_tutorials/scripts/hrp2017_gazebo_setup.py
new file mode 100755
index 00000000..9c47a6a0
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/hrp2017_gazebo_setup.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+#import rospkg
+#import sys
+#sys.path.append(rospkg.RosPack().get_path("jsk_hrp2_ros_bridge")+"/scripts")
+from jsk_hrp2_gazebo_setup import *
+
+def setupHrpsysConfigurator():
+ global hcf
+ #rtm.nshost = "localhost"
+ #rtm.nsport = "2809"
+ hcf=JSKHRP2RealHrpsysConfigurator("HRP2JSKNTS")
+ hcf.check_argument()
+
+if __name__ == '__main__':
+ setupHrpsysConfigurator()
diff --git a/hrpsys_gazebo_tutorials/scripts/hrp2017c_setup_gazebo.py b/hrpsys_gazebo_tutorials/scripts/hrp2017c_setup_gazebo.py
new file mode 100755
index 00000000..53d1d4c7
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/hrp2017c_setup_gazebo.py
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from jsk_hrp2_gazebo_setup import *
+
+def setupHrpsysConfigurator():
+ global hcf
+ rtm.nshost = "localhost"
+ hcf=JSKHRP2RealHrpsysConfigurator("HRP2JSKNTS")
+ hcf.check_argument()
+
+if __name__ == '__main__':
+ setupHrpsysConfigurator()
diff --git a/hrpsys_gazebo_tutorials/scripts/jsk_hrp2_gazebo_dashboard b/hrpsys_gazebo_tutorials/scripts/jsk_hrp2_gazebo_dashboard
new file mode 100755
index 00000000..260ad583
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/jsk_hrp2_gazebo_dashboard
@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+
+import sys
+
+import roslib
+pkg = 'hrpsys_gazebo_tutorials'
+
+import imp
+try:
+ imp.find_module(pkg)
+except:
+ roslib.load_manifest(pkg)
+
+from rqt_gui.main import Main
+
+import hrpsys_gazebo_tutorials
+import hrpsys_gazebo_tutorials.jsk_hrp2_gazebo_dashboard
+
+main = Main()
+sys.exit(main.main(sys.argv, standalone=pkg))
diff --git a/hrpsys_gazebo_tutorials/scripts/kill-ros-bridge.sh b/hrpsys_gazebo_tutorials/scripts/kill-ros-bridge.sh
new file mode 100755
index 00000000..f1e5b075
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/kill-ros-bridge.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source ~/.bashrc
+
+pgrep -f roslaunch.*hrp$(expr $HRP2NO + 2000)_gazebo.launch | xargs -n 1 kill
+#pgrep -f roslaunch.*hrp$(expr $HRP2NO + 2000)_ros_bridge.launch | xargs -n 1 kill
+pgrep -f roslaunch.*hrp$(expr $HRP2NO + 2000)_auditor_gazebo.launch | xargs -n 1 kill
diff --git a/hrpsys_gazebo_tutorials/scripts/replace_xmls.py b/hrpsys_gazebo_tutorials/scripts/replace_xmls.py
new file mode 100755
index 00000000..769b3f52
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/replace_xmls.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+
+import sys, os
+from xml.dom.minidom import parse, parseString
+import xml.dom
+import yaml
+import argparse
+
+reload(sys)
+sys.setdefaultencoding('utf-8')
+
+def getParentNode(node, depth):
+ if depth == 0:
+ return node
+ else:
+ return getParentNode(node.parentNode, depth - 1)
+
+if __name__ == '__main__':
+
+ parser = argparse.ArgumentParser(description='replace_xmls')
+ parser.add_argument('filename', nargs=1)
+ parser.add_argument('-O', '--output', help='output filename')
+ parser.add_argument('-C', '--config', help='config filename (yaml file)')
+
+ args = parser.parse_args()
+ obj = xml.dom.minidom.parse(args.filename[0])
+
+ if obj:
+ if args.config:
+ yaml_data = yaml.load(open(args.config).read())
+ if 'replace_xmls' in yaml_data:
+ for replace_xml in yaml_data['replace_xmls']:
+ match_rule = replace_xml['match_rule']
+ target_tags = []
+ if match_rule.has_key('tag'):
+ tags = obj.getElementsByTagName(match_rule['tag'])
+ if match_rule.has_key('attribute'):
+ attributes = match_rule['attribute']
+ for attribute in attributes:
+ attribute_name = str(attribute['name'])
+ if attribute.has_key('value'):
+ attribute_value = str(attribute['value'])
+ tags = [tag for tag in tags if tag.getAttribute(attribute_name) == attribute_value]
+ else:
+ for tag in tags:
+ sys.stderr.write('%s\n' % tag.hasAttribute(attribute_name))
+ tags = [tag for tag in tags if tag.hasAttribute(attribute_name)]
+
+ if match_rule.has_key('sub_tag'):
+ if match_rule.has_key('sub_attribute'):
+ sub_attributes = match_rule['sub_attribute']
+ for sub_attribute in sub_attributes:
+ sub_attribute_name = str(sub_attribute['name'])
+ if sub_attribute.has_key('value'):
+ sub_attribute_value = str(sub_attribute['value'])
+ matched_tags = []
+ for tag in tags:
+ for sub_tag in tag.getElementsByTagName(match_rule['sub_tag']):
+ if sub_tag.getAttribute(sub_attribute_name) == sub_attribute_value:
+ matched_tags.append(tag)
+ break
+ tags = matched_tags
+ else:
+ matched_tags = []
+ for tag in tags:
+ for sub_tag in tag.getElementsByTagName(match_rule['sub_tag']):
+ if sub_tag.hasAttribute(sub_attribute_name):
+ matched_tags.append(tag)
+ break
+ tags = matched_tags
+ else:
+ tags = [tag for tag in tags if len(tag.getElementsByTagName(match_rule['sub_tag'])) > 0]
+
+ if match_rule.has_key('parent_tag'):
+ if match_rule.has_key('parent_attribute'):
+ parent_attributes = match_rule['parent_attribute']
+ for parent_attribute in parent_attributes:
+ parent_attribute_name = str(parent_attribute['name'])
+ if parent_attribute.has_key('value'):
+ parent_attribute_value = str(parent_attribute['value'])
+ parent_depth = 1
+ if match_rule.has_key("parent_depth"):
+ parent_depth = match_rule["parent_depth"]
+ tags = [tag for tag in tags if getParentNode(tag, parent_depth).tagName == match_rule['parent_tag'] and getParentNode(tag, parent_depth).getAttribute(parent_attribute_name) == parent_attribute_value]
+ else:
+ parent_depth = 1
+ if match_rule.has_key("parent_depth"):
+ parent_depth = match_rule["parent_depth"]
+ tags = [tag for tag in tags if getParentNode(tag, parent_depth).tagName == match_rule['parent_tag'] and getParentNode(tag, parent_depth).hasAttribute(parent_attribute_name)]
+ else:
+ tags = [tag for tag in tags if getParentNode(tag, parent_depth).tagName == match_rule['parent_tag']]
+
+ target_tags = tags
+
+ else:
+ raise Exception("yaml does not have tag section")
+
+ if len(target_tags) > 0:
+ for tag in target_tags:
+ parent = tag.parentNode
+ # remove the tag
+ if replace_xml.has_key('replaced_xml'):
+ parent.removeChild(tag)
+ if str(replace_xml['replaced_xml']) != "":
+ parent.appendChild(parseString(str(replace_xml['replaced_xml'])).documentElement)
+ elif replace_xml.has_key('replaced_attribute_name') and replace_xml.has_key('replaced_attribute_value'):
+ tag.setAttribute(str(replace_xml['replaced_attribute_name']),
+ str(replace_xml['replaced_attribute_value']))
+ elif replace_xml.has_key('added_xml'):
+ if str(replace_xml['added_xml']) != "":
+ tag.appendChild(parseString(str(replace_xml['added_xml'])).documentElement)
+ else:
+ raise Exception("No rule to replacement is specified")
+ else:
+ sys.stderr.write('no configuration file !\n')
+
+ if args.output:
+ f = open(args.output, 'wb')
+ f.write(obj.toprettyxml('\t'))
+ f.close()
+ else:
+ sys.stdout.write(obj.toprettyxml('\t')) ## wirting to standard output
diff --git a/hrpsys_gazebo_tutorials/scripts/rtcd_start_command.sh b/hrpsys_gazebo_tutorials/scripts/rtcd_start_command.sh
new file mode 100755
index 00000000..89c92f1a
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/scripts/rtcd_start_command.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+
+FNAME=/tmp/modelloader_${USER}_`date +%Y-%m-%d_%H%M%S`.log
+rtmlaunch hrpsys_gazebo_tutorials modelloader.launch 2>&1 | tee ${FNAME} &
+
+sleep 5
+
+FNAME=/tmp/rtcd_${USER}_`date +%Y-%m-%d_%H%M%S`.log
+TMP_RTC_LIST_SETTING=$(rosrun hrpsys_gazebo_tutorials hrp`expr $HRP2NO + 2000`_gazebo_setup.py --getRTCList | grep "example" | grep "config_file")
+
+hrpsys_gazebo_tutorials_PATH=$(rospack find hrpsys_gazebo_tutorials)
+rosrun hrpsys_gazebo_tutorials hrp`expr $HRP2NO + 2000`_gazebo_setup.py --init 2>&1 | tee /tmp/jsk_hrp2_setup_${USER}.log & bash -c "roslaunch hrpsys_gazebo_tutorials rtcd.launch ARGS:=\"-f ${hrpsys_gazebo_tutorials_PATH}/config/rtcdRobotModeHRP2Common.conf ${TMP_RTC_LIST_SETTING}\" 2>&1 | tee ${FNAME};"
diff --git a/hrpsys_gazebo_tutorials/setup.py b/hrpsys_gazebo_tutorials/setup.py
new file mode 100644
index 00000000..33d79b7e
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/setup.py
@@ -0,0 +1,12 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+ packages=['hrpsys_gazebo_tutorials'],
+ package_dir={'': 'src'})
+
+
+setup(**setup_args)
diff --git a/hrpsys_gazebo_tutorials/src/hrpsys_gazebo_tutorials/__init__.py b/hrpsys_gazebo_tutorials/src/hrpsys_gazebo_tutorials/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/hrpsys_gazebo_tutorials/src/hrpsys_gazebo_tutorials/jsk_hrp2_gazebo_dashboard.py b/hrpsys_gazebo_tutorials/src/hrpsys_gazebo_tutorials/jsk_hrp2_gazebo_dashboard.py
new file mode 100755
index 00000000..3737f9bd
--- /dev/null
+++ b/hrpsys_gazebo_tutorials/src/hrpsys_gazebo_tutorials/jsk_hrp2_gazebo_dashboard.py
@@ -0,0 +1,36 @@
+from jsk_hrp2_ros_bridge.jsk_hrp2_dashboard import *
+import std_msgs.msg
+import rospkg
+hrpsys_gazebo_tutorials_path=rospkg.RosPack().get_path("hrpsys_gazebo_tutorials")
+
+class CraneButtonMenu(MenuDashWidget):
+ def __init__(self,topicname="CranePlugin",cranename="Crane"):
+ icons = [['bg-grey.svg', 'ic-runstop-off.svg'],
+ ['bg-green.svg', 'ic-runstop-on.svg'],
+ ['bg-red.svg', 'ic-runstop-off.svg']]
+ super(CraneButtonMenu, self).__init__('set Crane', icons)
+ self.update_state(0)
+ self.add_action(cranename + ' up', self.on_up)
+ self.add_action(cranename + ' down', self.on_down)
+ self.setFixedSize(self._icons[0].actualSize(QSize(50, 30)))
+ self.lift_command_pub = rospy.Publisher("/HRP2JSKNTS/" + topicname + "/LiftCommand", std_msgs.msg.Empty, queue_size = 1)
+ self.lower_command_pub = rospy.Publisher("/HRP2JSKNTS/" + topicname + "/LowerCommand", std_msgs.msg.Empty, queue_size = 1)
+ def on_up(self):
+ self.lift_command_pub.publish(std_msgs.msg.Empty())
+ def on_down(self):
+ self.lower_command_pub.publish(std_msgs.msg.Empty())
+
+class JSKHRP2gazeboDashboard(JSKHRP2Dashboard):
+ def setup(self, context):
+ super(JSKHRP2gazeboDashboard,self).setup(context)
+ self.name = "JSK hrp2 gazebo dashboard"
+ rospy.set_param("hrpsys_config_path", hrpsys_gazebo_tutorials_path+"/scripts")
+ rospy.set_param("hrpsys_config_script_name", "jsk_hrp2_gazebo_setup")
+ rospy.set_param("hrpsys_nshost", "localhost")
+ self._crane_button = CraneButtonMenu()
+ self._chest_crane_button = CraneButtonMenu("ChestCranePlugin","ChestCrane")
+ def get_widgets(self):
+ ret = super(JSKHRP2gazeboDashboard,self).get_widgets()
+ ret[-2].append(self._crane_button)
+ ret[-2].append(self._chest_crane_button)
+ return ret
diff --git a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml
index 4258f589..8a158c02 100644
--- a/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml
+++ b/hrpsys_ros_bridge_tutorials/models/hrp2jsknts.yaml
@@ -164,27 +164,167 @@ replace_xmls:
tag: inertia
attribute_name: ixx
attribute_value: '0'
- replaced_xml: ''
+ 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 '
+ attribute_value: RLEG_LINK0
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RLEG_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RLEG_LINK2
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RLEG_LINK3
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RLEG_LINK4
+ replaced_xml: '\n 0.9\n 0.9\n true\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 '
+ 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 true\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 true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LLEG_LINK0
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LLEG_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LLEG_LINK2
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LLEG_LINK3
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LLEG_LINK4
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - 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 true\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 '
+ 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 true\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 '
+ attribute_value: BODY
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: CHEST_LINK0
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: CHEST_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: HEAD_LINK0
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: HEAD_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK0
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK2
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK3
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK4
+ replaced_xml: '\n 0.9\n 0.9\n false\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_LINK5
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: RARM_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 false\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_LINK1
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_LINK2
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_LINK3
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_LINK4
+ replaced_xml: '\n 0.9\n 0.9\n false\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_LINK5
+ replaced_xml: '\n 0.9\n 0.9\n true\n '
+ - match_rule:
+ tag: gazebo
+ attribute_name: reference
+ attribute_value: LARM_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 false\n '
- match_rule:
tag: limit
attribute_name: velocity
@@ -195,6 +335,22 @@ replace_xmls:
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: 10
+ - match_rule:
+ tag: cfmDamping
+ parent_depth: 2
+ parent_attribute_name: name
+ parent_attribute_value: HRP2JSKNTS
+ replaced_xml: 'true'
# for hands
- match_rule:
tag: inertia
@@ -236,32 +392,32 @@ replace_xmls:
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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\n '
- match_rule:
tag: inertia
parent_depth: 2
@@ -302,29 +458,29 @@ replace_xmls:
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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\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 '
+ 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 true\n '