Merge pull request #90 from mmurooka/move-hrpsys-gazebo-tutorials-to-…

…rtmros-tutorials Move hrpsys_gazebo_tutorials to rtmros_tutorials
start-jsk · Oct 13, 2014 · 4658cd9 · 4658cd9
2 parents a762d3a + 2857f8f
commit 4658cd9
Show file tree

Hide file tree

Showing 147 changed files with 10,303 additions and 0 deletions.
diff --git a/hrpsys_gazebo_tutorials/CMakeLists.txt b/hrpsys_gazebo_tutorials/CMakeLists.txt
@@ -0,0 +1,68 @@
+if(NOT USE_ROSBUILD)
+  include(catkin.cmake)
+  return()
+endif()
+
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+
+macro (generate_gazebo_urdf_file _robot_name)
+  set(_out_dir "${PROJECT_SOURCE_DIR}/robot_models/${_robot_name}")
+  set(_out_urdf_file "${_out_dir}/${_robot_name}.urdf")
+  add_custom_command(OUTPUT ${_out_dir}/meshes
+    COMMAND mkdir ${_out_dir}/meshes)
+  add_custom_command(OUTPUT ${_out_dir}/hrpsys
+    COMMAND mkdir ${_out_dir}/hrpsys)
+  add_custom_command(OUTPUT ${_out_urdf_file}
+    COMMAND ${PROJECT_SOURCE_DIR}/robot_models/install_robot_common.sh ${_robot_name}
+    DEPENDS ${_out_dir}/hrpsys ${_out_dir}/meshes)
+  add_custom_target(${_robot_name}_compile DEPENDS ${_out_urdf_file})
+  set(ROBOT ${_robot_name})
+  string(TOLOWER ${_robot_name} _sname)
+  #configure_file(${PROJECT_SOURCE_DIR}/scripts/default_robot_hrpsys_bringup.launch.in ${PROJECT_SOURCE_DIR}/build/${_sname}_hrpsys_bringup.launch)
+  if(NOT EXISTS ${PROJECT_SOURCE_DIR}/launch/gazebo_${_sname}_no_controllers.launch)
+    configure_file(${PROJECT_SOURCE_DIR}/scripts/default_gazebo_robot_no_controllers.launch.in ${PROJECT_SOURCE_DIR}/launch/gazebo_${_sname}_no_controllers.launch)
+  endif()
+  list(APPEND compile_robots ${_robot_name}_compile)
+endmacro()
+
+generate_gazebo_urdf_file(SampleRobot)
+generate_gazebo_urdf_file(HRP3HAND_L)
+generate_gazebo_urdf_file(HRP3HAND_R)
+generate_gazebo_urdf_file(HRP2JSK)
+generate_gazebo_urdf_file(HRP2JSKNT)
+generate_gazebo_urdf_file(HRP2JSKNTS)
+generate_gazebo_urdf_file(STARO)
+generate_gazebo_urdf_file(HRP4C)
+
+add_custom_target(all_robots_compile ALL DEPENDS ${compile_robots})
+
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
diff --git a/hrpsys_gazebo_tutorials/Makefile b/hrpsys_gazebo_tutorials/Makefile
@@ -0,0 +1,2 @@
+EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1
+include $(shell rospack find mk)/cmake.mk
diff --git a/hrpsys_gazebo_tutorials/README.md b/hrpsys_gazebo_tutorials/README.md
@@ -0,0 +1,7 @@
+## How to move SampleRobot in Gazebo with hrpsys-base
+1. confirm if there is a SampleRobot.urdf in ```hrpsys_gazebo_tutorials/robot_models/SampleRobot```
+1. ```source `rospack find hrpsys_gazebo_tutorials`/setup.sh; roslaunch hrpsys_gazebo_tutorials gazebo_samplerobot_no_controllers.launch```
+1. ```rtmlaunch hrpsys_gazebo_tutorials samplerobot_hrpsys_bringup.launch```
+1. ```roscd hrpsys_ros_bridge_tutorials/euslisp; roseus samplerobot-interface.l "(samplerobot-init)" "(objects (list *sr*))" "(send *ri* :angle-vector (send *sr* :angle-vector) 5000)"```
+
+Do not forget to type ```pkill gzserver``` in addition to Ctrl-c in order to kill gazebo.
diff --git a/hrpsys_gazebo_tutorials/catkin.cmake b/hrpsys_gazebo_tutorials/catkin.cmake
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hrpsys_gazebo_tutorials)
+
+find_package(catkin REQUIRED COMPONENTS euscollada hrpsys_ros_bridge hrpsys_ros_bridge_tutorials)
+
+set(PKG_CONFIG_PATH ${hrpsys_PREFIX}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH})
+find_package(PkgConfig)
+pkg_check_modules(openhrp3 openhrp3.1 REQUIRED)
+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)
+
+if(NOT hrpsys_ros_bridge_tutorials_SOURCE_DIR)
+  execute_process(
+    COMMAND rospack find hrpsys_ros_bridge_tutorials
+    OUTPUT_VARIABLE hrpsys_ros_bridge_tutorials_SOURCE_DIR)
+  string(REGEX REPLACE "\n" "" hrpsys_ros_bridge_tutorials_SOURCE_DIR ${hrpsys_ros_bridge_tutorials_SOURCE_DIR})
+endif()
+
+## Convert robot models
+if(EXISTS ${hrpsys_gazebo_general_SOURCE_DIR})
+  set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_SOURCE_DIR})
+else()
+  set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_PREFIX}/share/hrpsys_gazebo_general)
+endif()
+include(${hrpsys_gazebo_general_PACKAGE_PATH}/cmake/compile_robot_model_for_gazebo.cmake)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/SampleRobot.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP3HAND_L.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP3HAND_R.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP2JSK.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP2JSKNT.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP2JSKNTS.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/STARO.dae)
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_tutorials_SOURCE_DIR}/models/HRP4C.dae)
+add_custom_target(all_robots_compile_tutorials ALL DEPENDS ${compile_urdf_robots})
+
+## install
+install(FILES setup.sh
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+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)
diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSK.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSK.yaml
@@ -0,0 +1,93 @@
+hrpsys_gazebo_configuration:
+## velocity feedback for joint control, use parameter gains/joint_name/p_v
+  use_velocity_feedback: true
+## synchronized hrpsys and gazebo
+# use_synchronized_command: false
+# name of robot (using for namespace)
+  robotname: HRP2JSK
+# 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]
+# joints list used in gazebo, sizeof(joint_id_list) == sizeof(joints)
+  joints:
+    - RLEG_JOINT0
+    - RLEG_JOINT1
+    - RLEG_JOINT2
+    - RLEG_JOINT3
+    - RLEG_JOINT4
+    - RLEG_JOINT5
+    - LLEG_JOINT0
+    - LLEG_JOINT1
+    - LLEG_JOINT2
+    - LLEG_JOINT3
+    - LLEG_JOINT4
+    - LLEG_JOINT5
+    - CHEST_JOINT0
+    - CHEST_JOINT1
+    - HEAD_JOINT0
+    - HEAD_JOINT1
+    - RARM_JOINT0
+    - RARM_JOINT1
+    - RARM_JOINT2
+    - RARM_JOINT3
+    - RARM_JOINT4
+    - RARM_JOINT5
+    - RARM_JOINT6
+    - RARM_JOINT7
+    - LARM_JOINT0
+    - LARM_JOINT1
+    - LARM_JOINT2
+    - LARM_JOINT3
+    - LARM_JOINT4
+    - LARM_JOINT5
+    - LARM_JOINT6
+    - LARM_JOINT7
+  gains:
+    CHEST_JOINT0: {p: 3000.0, d:   8.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    CHEST_JOINT1: {p: 1000.0, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    HEAD_JOINT0:  {p:  150.0, d:   0.5, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    HEAD_JOINT1:  {p:  700.0, d:   1.5, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT0:  {p: 2000.0, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT1:  {p: 1000.0, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT2:  {p:  200.0, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT3:  {p: 1500.0, d:   3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT4:  {p:  200.0, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT5:  {p:  400.0, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT6:  {p:  400.0, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LARM_JOINT7:  {p:   20.0, d:  0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT0:  {p: 2000.0, d:   4.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT1:  {p: 1000.0, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT2:  {p:  200.0, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT3:  {p: 1500.0, d:   3.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT4:  {p:  200.0, d:   0.4, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT5:  {p:  400.0, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT6:  {p:  400.0, d:   0.8, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RARM_JOINT7:  {p:   20.0, d:  0.02, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    LLEG_JOINT0:  {p: 1500.0, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0} # CRTOCH-Y
+    LLEG_JOINT1:  {p: 2000.0, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-R
+    LLEG_JOINT2:  {p: 3800.0, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0} # CRTOCH-P
+    LLEG_JOINT3:  {p: 3400.0, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # KNEE-P
+    LLEG_JOINT4:  {p: 2800.0, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-P
+    LLEG_JOINT5:  {p: 2000.0, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0} # ANKLE-R
+    RLEG_JOINT0:  {p: 1500.0, d:   2.0, i: 0.0, i_clamp: 0.0, p_v: 350.0}
+    RLEG_JOINT1:  {p: 2000.0, d:   4.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
+    RLEG_JOINT2:  {p: 3800.0, d:  15.0, i: 0.0, i_clamp: 0.0, vp: 4.0, p_v: 350.0}
+    RLEG_JOINT3:  {p: 3400.0, d:  12.5, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
+    RLEG_JOINT4:  {p: 2800.0, d:  10.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
+    RLEG_JOINT5:  {p: 2000.0, d:   2.0, i: 0.0, i_clamp: 0.0, vp: 2.0, p_v: 350.0}
+
+  force_torque_sensors:
+    - rfsensor
+    - lfsensor
+    - rhsensor
+    - lhsensor
+  force_torque_sensors_config:
+## 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_config:
+## TODO: add translation and rotation
+    imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'}
diff --git a/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml b/hrpsys_gazebo_tutorials/config/HRP2JSKNT.yaml
@@ -0,0 +1,98 @@
+hrpsys_gazebo_configuration:
+## velocity feedback for joint control, use parameter gains/joint_name/p_v
+  use_velocity_feedback: true
+## synchronized hrpsys and gazebo
+# use_synchronized_command: false
+# name of robot (using for namespace)
+  robotname: HRP2JSKNT
+# joint_id (order) conversion from gazebo to hrpsys, joint_id_list[gazebo_id] := hrpsys_id
+  joint_id_list: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33]
+# joints list used in gazebo, sizeof(joint_id_list) == sizeof(joints)
+  joints:
+    - RLEG_JOINT0
+    - RLEG_JOINT1
+    - RLEG_JOINT2
+    - RLEG_JOINT3
+    - RLEG_JOINT4
+    - RLEG_JOINT5
+    - RLEG_JOINT6
+    - LLEG_JOINT0
+    - LLEG_JOINT1
+    - LLEG_JOINT2
+    - LLEG_JOINT3
+    - LLEG_JOINT4
+    - LLEG_JOINT5
+    - LLEG_JOINT6
+    - CHEST_JOINT0
+    - CHEST_JOINT1
+    - HEAD_JOINT0
+    - HEAD_JOINT1
+    - RARM_JOINT0
+    - RARM_JOINT1
+    - RARM_JOINT2
+    - RARM_JOINT3
+    - RARM_JOINT4
+    - RARM_JOINT5
+    - RARM_JOINT6
+    - RARM_JOINT7
+    - LARM_JOINT0
+    - LARM_JOINT1
+    - LARM_JOINT2
+    - LARM_JOINT3
+    - LARM_JOINT4
+    - LARM_JOINT5
+    - LARM_JOINT6
+    - LARM_JOINT7
+## 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}
+    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: 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}
+
+  force_torque_sensors:
+    - rfsensor
+    - lfsensor
+    - rhsensor
+    - lhsensor
+  force_torque_sensors_config:
+## 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_config:
+## TODO: add translation and rotation
+    imu_sensor0: {ros_name: 'waist_imu', link_name: 'CHEST_LINK1', frame_id: 'CHEST_LINK1'}