Load correct libhrpIo.so from RobotHardware.so

hrpsys_gazebo_general/catkin.cmake

@@ -58,6 +58,26 @@ add_dependencies(AddForcePlugin hrpsys_gazebo_msgs_gencpp)
 add_dependencies(GetVelPlugin hrpsys_gazebo_msgs_gencpp)
 #endif()
 
+# move libhrpIo.so
+# lib -> {source}/lib
+if(NOT EXISTS ${PROJECT_SOURCE_DIR}/lib/)
+  execute_process(
+    COMMAND cmake -E make_directory ${PROJECT_SOURCE_DIR}/lib/
+    RESULT_VARIABLE _make_failed)
+  if (_make_failed)
+    message(FATAL_ERROR "make_directory ${PROJECT_SOURCE_DIR}/lib/ failed: ${_make_failed}")
+  endif(_make_failed)
+endif()
+if(EXISTS ${CATKIN_DEVEL_PREFIX}/share/hrpsys_gazebo_general/lib/libhrpIo.so)
+  execute_process(
+    COMMAND cmake -E rename ${CATKIN_DEVEL_PREFIX}/share/hrpsys_gazebo_general/lib/libhrpIo.so ${PROJECT_SOURCE_DIR}/lib/libhrpIo.so
+    RESULT_VARIABLE _rename_failed)
+  message("move libhrpIo.so ${PROJECT_SOURCE_DIR}/lib/libhrpIo.so")
+  if (_rename_failed)
+    message(FATAL_ERROR "Move libhrpIo.so failed: ${_rename_failed}")
+  endif(_rename_failed)
+endif()
+
 install(DIRECTORY
   ${CATKIN_DEVEL_PREFIX}/share/hrpsys_gazebo_general/lib
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

hrpsys_gazebo_tutorials/catkin.cmake

@@ -1,8 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(hrpsys_gazebo_tutorials)
 
-#find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge_tutorials collada_tools euscollada)
-find_package(catkin REQUIRED COMPONENTS collada_tools euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials)
+find_package(catkin REQUIRED COMPONENTS collada_urdf_jsk_patch euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials)
 
 set(PKG_CONFIG_PATH ${hrpsys_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH})
 find_package(PkgConfig)
@@ -12,7 +11,7 @@ pkg_check_modules(hrpsys hrpsys-base REQUIRED)
 
 catkin_package(
     DEPENDS
-    CATKIN_DEPENDS collada_tools euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials
+    CATKIN_DEPENDS collada_urdf_jsk_patch euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials
     INCLUDE_DIRS # TODO include
     LIBRARIES # TODO
 )
@@ -45,8 +44,8 @@ macro (generate_gazebo_urdf_file _robot_name)
   # ${compile_robots} is a global target used in compile_robot_model.cmake of hrpsys_ros_bridge.
   # this dependency means that run install_robot_common.sh after executing all of ${compile_robots}.
     add_custom_command(OUTPUT ${_out_urdf_file}
-      COMMAND ${PROJECT_SOURCE_DIR}/robot_models/install_robot_common.sh ${_robot_name} ${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models  ${hrpsys_gazebo_tutorials_SOURCE_DIR}/robot_models/${_robot_name} ${collada_tools_PREFIX}/lib/collada_tools/collada_to_urdf ${PROJECT_SOURCE_DIR}/..
-      DEPENDS ${PROJECT_SOURCE_DIR}/robot_models/install_robot_common.sh ${_out_dir}/hrpsys ${_out_dir}/meshes ${collada_tools_PREFIX}/lib/collada_tools/collada_to_urdf ${compile_robots})
+      COMMAND ${PROJECT_SOURCE_DIR}/robot_models/install_robot_common.sh ${_robot_name} ${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models  ${hrpsys_gazebo_tutorials_SOURCE_DIR}/robot_models/${_robot_name} ${collada_urdf_jsk_patch_PREFIX}/lib/collada_urdf_jsk_patch/collada_to_urdf ${PROJECT_SOURCE_DIR}/..
+      DEPENDS ${PROJECT_SOURCE_DIR}/robot_models/install_robot_common.sh ${_out_dir}/hrpsys ${_out_dir}/meshes ${collada_urdf_jsk_patch_PREFIX}/lib/collada_urdf_jsk_patch/collada_to_urdf ${compile_robots})
   add_custom_target(${_robot_name}_compile DEPENDS ${_out_urdf_file})
   set(ROBOT ${_robot_name})
   string(TOLOWER ${_robot_name} _sname)

hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml

@@ -45,53 +45,54 @@ hrpsys_gazebo_configuration:
     - LARM_JOINT7
 ## jointid:
   gains:
-    CHEST_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    CHEST_JOINT1: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    HEAD_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    HEAD_JOINT1: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT1: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT2: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT3: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT4: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT5: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT6: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LARM_JOINT7: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT1: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT2: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT3: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT4: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT5: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT6: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RARM_JOINT7: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT1: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT2: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT3: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT4: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT5: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT6: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RLEG_JOINT0: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    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}
+    LARM_JOINT7: {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}
+    RARM_JOINT7: {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: 350.0}
-    RLEG_JOINT3: {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: 350.0}
-##
+    RLEG_JOINT6: {p: 700.0, d:   0.075, i: 0.0, i_clamp: 0.0, p_v: 250.0}
+
   force_torque_sensors:
+    - rfsensor
     - lfsensor
-    - rfsensor 
+    - rhsensor
     - lhsensor
-    - rhsensor 
   force_torque_sensors_config:
-    lfsensor: {joint_name: 'LLEG_JOINT6', frame_id: 'LLEG_LINK6'}
-    rfsensor: {joint_name: 'RLEG_JOINT6', frame_id: 'RLEG_LINK6'}
-    lhsensor: {joint_name: 'LARM_JOINT6', frame_id: 'LARM_LINK6'}
-    rhsensor: {joint_name: 'RARM_JOINT6', frame_id: 'RARM_LINK6'}
-##
+## TODO: add translation and rotation
+    lfsensor: {joint_name: 'LLEG_JOINT5', frame_id: 'LLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]}
+    rfsensor: {joint_name: 'RLEG_JOINT5', frame_id: 'RLEG_LINK5', translation: [0, 0, -0.105], rotation: [1, 0, 0, 0]}
+    lhsensor: {joint_name: 'LARM_JOINT6', frame_id: 'LARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]}
+    rhsensor: {joint_name: 'RARM_JOINT6', frame_id: 'RARM_LINK6', translation: [0, 0, -0.0775], rotation: [1, 0, 0, 0]}
   imu_sensors:
     - imu_sensor0
-  imu_sensors:
-    imu_sensor0: {name: 'waist_imu', link_name: 'CHEST_LINK0', frame_id: 'WAIST_LINK0'}
+  imu_sensors_config:
+## TODO: add translation and rotation
+    imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'}

hrpsys_gazebo_tutorials/config/HRP2JSKNTS_controller_config_for_moveit.yaml

@@ -0,0 +1,51 @@
+controller_configuration:
+  - group_name: larm
+    controller_name: /larm_controller
+    joint_list:
+      - LARM_JOINT0
+      - LARM_JOINT1
+      - LARM_JOINT2
+      - LARM_JOINT3
+      - LARM_JOINT4
+      - LARM_JOINT5
+      - LARM_JOINT6
+  - group_name: rarm
+    controller_name: /rarm_controller
+    joint_list:
+      - RARM_JOINT0
+      - RARM_JOINT1
+      - RARM_JOINT2
+      - RARM_JOINT3
+      - RARM_JOINT4
+      - RARM_JOINT5
+      - RARM_JOINT6
+  - group_name: lleg
+    controller_name: /lleg_controller
+    joint_list:
+      - LLEG_JOINT0
+      - LLEG_JOINT1
+      - LLEG_JOINT2
+      - LLEG_JOINT3
+      - LLEG_JOINT4
+      - LLEG_JOINT5
+      - LLEG_JOINT6
+  - group_name: rleg
+    controller_name: /rleg_controller
+    joint_list:
+      - RLEG_JOINT0
+      - RLEG_JOINT1
+      - RLEG_JOINT2
+      - RLEG_JOINT3
+      - RLEG_JOINT4
+      - RLEG_JOINT5
+      - RLEG_JOINT6
+  - group_name: torso
+    controller_name: /torso_controller
+    joint_list:
+      - CHEST_JOINT0
+      - CHEST_JOINT1
+  - group_name: head
+    controller_name: /head_controller
+    joint_list:
+      - HEAD_JOINT0
+      - HEAD_JOINT1

hrpsys_gazebo_tutorials/config/STARO.yaml

@@ -42,38 +42,38 @@ hrpsys_gazebo_configuration:
     - LLEG_JOINT4
     - LLEG_JOINT5
   gains:
-    CHEST_JOINT0: {p: 300000, d:   8.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    CHEST_JOINT1: {p: 100000, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    HEAD_JOINT0:  {p:  15000, d:   0.5, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    HEAD_JOINT1:  {p:  70000, d:   1.5, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT0:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT1:  {p: 100000, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT2:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT3:  {p: 150000, d:   3.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT4:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT5:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT6:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RARM_JOINT7:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT0:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT1:  {p: 100000, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT2:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT3:  {p: 150000, d:   3.0, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT4:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT5:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT6:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    LARM_JOINT7:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 200.0}
-    RLEG_JOINT0:  {p: 150000, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    RLEG_JOINT1:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
-    RLEG_JOINT2:  {p: 380000, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
-    RLEG_JOINT3:  {p: 340000, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
-    RLEG_JOINT4:  {p: 280000, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
-    RLEG_JOINT5:  {p: 200000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
-    LLEG_JOINT0:  {p: 150000, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
-    LLEG_JOINT1:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
-    LLEG_JOINT2:  {p: 380000, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
-    LLEG_JOINT3:  {p: 340000, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
-    LLEG_JOINT4:  {p: 280000, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
-    LLEG_JOINT5:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
+    CHEST_JOINT0: {p: 300000, d:   8.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    CHEST_JOINT1: {p: 100000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    HEAD_JOINT0:  {p:  15000, d:   0.5, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    HEAD_JOINT1:  {p:  70000, d:   1.5, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT0:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT1:  {p: 100000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT2:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT3:  {p: 150000, d:   3.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT4:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT5:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT6:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RARM_JOINT7:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT0:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT1:  {p: 100000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT2:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT3:  {p: 150000, d:   3.0, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT4:  {p:  20000, d:   0.4, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT5:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT6:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    LARM_JOINT7:  {p:  40000, d:   0.8, i: 0.0, i_clamp: 0.0, vp: 1.0, p_v: 140.0}
+    RLEG_JOINT0:  {p: 150000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 250.0}
+    RLEG_JOINT1:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 250.0}
+    RLEG_JOINT2:  {p: 380000, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 8.0, p_v: 650.0}
+    RLEG_JOINT3:  {p: 340000, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 650.0}
+    RLEG_JOINT4:  {p: 280000, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 8.0, p_v: 250.0}
+    RLEG_JOINT5:  {p: 200000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 450.0}
+    LLEG_JOINT0:  {p: 150000, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 250.0}
+    LLEG_JOINT1:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 250.0}
+    LLEG_JOINT2:  {p: 380000, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 8.0, p_v: 650.0}
+    LLEG_JOINT3:  {p: 340000, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 8.0, p_v: 650.0}
+    LLEG_JOINT4:  {p: 280000, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 250.0}
+    LLEG_JOINT5:  {p: 200000, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 450.0}
 #   force_torque_sensors:
 #     - rfsensor
 #     - lfsensor

hrpsys_gazebo_tutorials/euslisp/hand-command-publisher.l

@@ -39,6 +39,45 @@
      (&optional l/r)
      (send self :hand-effort-vector (float-vector 0 0 0 0 0 0) l/r)
      )))
+(when (boundp 'hrp2jsknts-interface)
+  (defmethod hrp2jsknts-interface
+    (:init-hand
+     ()
+     (setq hcp (instance hand-command-publisher-class :init)))
+    (:hand-angle-vector
+     (av &optional l/r)
+     (cond ((or (equal l/r :lhand) (equal l/r :larm))
+	    (send hcp :send-command (list av nil)))
+	   ((or (equal l/r :rhand) (equal l/r :rarm))
+	    (send hcp :send-command (list nil av)))
+	   (t
+	    (send hcp :send-command av))
+	   ))
+    (:hand-angle-state-vector
+     (&optional l/r)
+     (cond ((or (equal l/r :lhand) (equal l/r :larm))
+	    (elt (send hcp :get-state) 0))
+	   ((or (equal l/r :rhand) (equal l/r :rarm))
+	    (elt (send hcp :get-state) 1))
+	   (t
+	    (send hcp :get-state))
+	   ))
+    (:hand-effort-vector
+     (ef &optional l/r)
+     (cond ((or (equal l/r :lhand) (equal l/r :larm))
+	    (send hcp :send-effort-command (list ef nil)))
+	   ((or (equal l/r :rhand) (equal l/r :rarm))
+	    (send hcp :send-effort-command (list nil ef)))
+	   (t
+	    (send hcp :send-effort-command (list ef ef)))
+	   ))
+    (:start-grasp
+     (&optional l/r)
+     (send self :hand-effort-vector (float-vector 0 -25 25 0 25 25) l/r))
+    (:stop-grasp
+     (&optional l/r)
+     (send self :hand-effort-vector (float-vector 0 0 0 0 0 0) l/r)
+     )))
 ;;(warn "~%")
 ;;(warn "(send *ri* (list (float-vector 10 20 30 40 50 60) (float-vector 50 40 30 20 10 0)))")
 ;;(warn "(send *ri* :get-state)~%")

hrpsys_gazebo_tutorials/euslisp/hrp2jsknt-interface-ex.l

@@ -17,6 +17,7 @@
   (setq *robot* *hrp2jsknt*)
   (objects (list *hrp2*))
   (send *irtviewer* :change-background #f(1 1 1)))
+
 (defun init
   (&key (real t))
   (hrp2jsknt-init-ex :real real))
@@ -34,8 +35,6 @@
   (send *ri* :angle-vector (send *robot* pose) wait-time)
   (when wait? (send *ri* :wait-interpolation)))
 
-
-
 (defun model2real
   ()
   (send *ri* :angle-vector (send *robot* :angle-vector)))

hrpsys_gazebo_tutorials/euslisp/hrp2jsknts-interface-ex.l

@@ -0,0 +1,40 @@
+(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")
+(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-utils.l")
+
+(load "package://hrpsys_gazebo_tutorials/euslisp/hand-command-publisher.l")
+(load "package://hrpsys_gazebo_tutorials/euslisp/hrp2jsknts-with-limb-trajectory.l")
+
+(defun hrp2jsknts-init-ex
+  (&key (real t))
+  (cond
+   (real
+    (hrp2jsknts-init)
+    (send *ri* :init-hand))
+   (t
+    (if (not (boundp '*hrp2jsknts*))
+	(setq *hrp2jsknts* (instance hrp2jsknts-robot :init)))))
+  (setq *hrp2* *hrp2jsknts*)
+  (setq *robot* *hrp2jsknts*)
+  (objects (list *hrp2*))
+  (send *irtviewer* :change-background #f(1 1 1)))
+
+(defun init
+  (&key (real t))
+  (hrp2jsknts-init-ex :real real))
+
+(defun reset-pose
+  (&key (wait-time 2000) (wait? t))
+  (send-special-pose :reset-pose :wait-time wait-time :wait? wait?))
+
+(defun reset-manip-pose
+  (&key (wait-time 2000) (wait? t))
+  (send-special-pose :reset-manip-pose :wait-time wait-time :wait? wait?))
+
+(defun send-special-pose
+  (pose &key (wait-time 2000) (wait? t))
+  (send *ri* :angle-vector (send *robot* pose) wait-time)
+  (when wait? (send *ri* :wait-interpolation)))
+
+(defun model2real
+  ()
+  (send *ri* :angle-vector (send *robot* :angle-vector)))