Move samplerobot to hrpsys_gazebo_general

hrpsys_gazebo_general/catkin.cmake

@@ -7,8 +7,17 @@ find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge hrpsys_gazebo_msgs)
 find_package(PkgConfig)
 pkg_check_modules(openrtm_aist openrtm-aist REQUIRED)
 pkg_check_modules(openhrp3 openhrp3.1 REQUIRED)
+pkg_check_modules(collada_urdf_jsk_patch collada_urdf_jsk_patch)
 catkin_package(CATKIN_DEPENDS hrpsys_ros_bridge hrpsys_gazebo_msgs)
 
+# set CMAKE_BUILD_TYPE
+if(NOT CMAKE_BUILD_TYPE)
+  set(
+    CMAKE_BUILD_TYPE Release CACHE STRING
+    "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel."
+    FORCE)
+endif()
+
 ## Build only gazebo iob
 find_package(PkgConfig)
 pkg_check_modules(omniorb omniORB4 REQUIRED)
@@ -28,6 +37,7 @@ link_directories(${CATKIN_DEVEL_PREFIX}/lib ${hrpsys_PREFIX}/lib ${openhrp3_LIBR
 add_subdirectory(iob)
 
 add_custom_target(hrpsys_gazebo_general_iob ALL DEPENDS RobotHardware_gazebo)
+add_dependencies(hrpsys_gazebo_general_iob hrpsys_gazebo_msgs_gencpp)
 
 ## Gazebo plugins
 include (FindPkgConfig)
@@ -42,17 +52,32 @@ link_directories( ${GAZEBO_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS})
 
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/plugins)
 
-#if ($ENV{ROS_DISTRO} STREQUAL "groovy")
 add_library(IOBPlugin src/IOBPlugin.cpp)
-add_library(SetVelPlugin src/SetVelPlugin.cpp)
-add_dependencies(hrpsys_gazebo_general_iob hrpsys_gazebo_msgs_gencpp)
 add_dependencies(IOBPlugin hrpsys_gazebo_msgs_gencpp)
+install(TARGETS IOBPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+add_library(SetVelPlugin src/SetVelPlugin.cpp)
 add_dependencies(SetVelPlugin hrpsys_gazebo_msgs_gencpp)
+install(TARGETS SetVelPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 add_library(AddForcePlugin src/AddForcePlugin.cpp)
-add_library(GetVelPlugin src/GetVelPlugin.cpp)
 add_dependencies(AddForcePlugin hrpsys_gazebo_msgs_gencpp)
+install(TARGETS AddForcePlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+add_library(GetVelPlugin src/GetVelPlugin.cpp)
 add_dependencies(GetVelPlugin hrpsys_gazebo_msgs_gencpp)
+install(TARGETS GetVelPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 add_library(ThermoPlugin src/ThermoPlugin.cpp)
 add_dependencies(ThermoPlugin hrpsys_gazebo_msgs_gencpp)
-#endif()
+install(TARGETS ThermoPlugin LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+## Convert robot models
+include(${PROJECT_SOURCE_DIR}/cmake/compile_robot_model_for_gazebo.cmake)
+if(EXISTS ${hrpsys_ros_bridge_SOURCE_DIR})
+  set(hrpsys_ros_bridge_PACKAGE_PATH ${hrpsys_ros_bridge_SOURCE_DIR})
+else()
+  set(hrpsys_ros_bridge_PACKAGE_PATH ${hrpsys_ros_bridge_PREFIX}/share/hrpsys_ros_bridge)
+endif()
+generate_gazebo_urdf_file(${hrpsys_ros_bridge_PACKAGE_PATH}/models/SampleRobot.dae)
+add_custom_target(all_robots_compile ALL DEPENDS ${compile_urdf_robots})
 
+## install
+install(DIRECTORY launch scripts worlds config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+install(PROGRAMS setup.sh DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

hrpsys_gazebo_general/cmake/compile_robot_model_for_gazebo.cmake

@@ -0,0 +1,53 @@
+##
+## define macros
+##
+macro (generate_gazebo_urdf_file daefile)
+  # set variable
+  if(hrpsys_gazebo_general_SOURCE_DIR)
+    set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_SOURCE_DIR})
+  elseif(hrpsys_gazebo_general_SOURCE_PREFIX)
+    set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_SOURCE_PREFIX})
+  else(hrpsys_gazebo_general_SOURCE_PREFIX)
+    set(hrpsys_gazebo_general_PACKAGE_PATH ${hrpsys_gazebo_general_PREFIX}/share/hrpsys_gazebo_general)
+  endif()
+  get_filename_component(_robot_name ${daefile} NAME_WE)
+  set(_workdir ${PROJECT_SOURCE_DIR}/robot_models)
+  set(_out_dir "${_workdir}/${_robot_name}")
+  set(_out_urdf_file "${_out_dir}/${_robot_name}.urdf")
+  set(_out_urdf_gazebo_file "${_out_dir}/${_robot_name}_gazebo.urdf")
+  add_custom_command(OUTPUT ${_out_dir}/meshes
+    COMMAND mkdir ${_out_dir}/meshes)
+  # convert dae to urdf
+  message("generate_gazebo_urdf_file ${daefile}")
+  ## ${compile_robots} is a global target used in compile_robot_model.cmake of hrpsys_ros_bridge.
+  ## this dependency means that converting urdf after executing all of ${compile_robots}.
+  add_custom_command(OUTPUT ${_out_urdf_file}
+    COMMAND ${collada_urdf_jsk_patch_PREFIX}/lib/collada_urdf_jsk_patch/collada_to_urdf ${daefile} -G -A --mesh_output_dir ${_out_dir}/meshes --mesh_prefix "package://${PROJECT_NAME}/robot_models/${_robot_name}/meshes" --output_file=${_out_urdf_file}
+    COMMAND ${_out_dir}/${_robot_name}_additional_urdf_setting.sh ${_out_urdf_file}
+    DEPENDS ${_out_dir}/meshes ${compile_robots})
+  add_custom_command(OUTPUT ${_out_urdf_file}
+    COMMAND ${collada_urdf_jsk_patch_PREFIX}/lib/collada_urdf_jsk_patch/collada_to_urdf ${daefile} -G -A --mesh_output_dir ${_out_dir}/meshes --mesh_prefix "package://${PROJECT_NAME}/robot_models/${_robot_name}/meshes" --output_file=${_out_urdf_file}
+    COMMAND ${_out_dir}/${_robot_name}_additional_urdf_setting.sh ${_out_urdf_file}
+    DEPENDS ${_out_dir}/meshes ${compile_robots})
+  add_custom_command(OUTPUT ${_out_urdf_gazebo_file}
+    COMMAND sed -e "s@package://${PROJECT_NAME}/robot_models/@model://@g" ${_out_urdf_file} > ${_out_urdf_gazebo_file}
+    DEPENDS ${_out_urdf_file})
+  add_custom_target(${_robot_name}_compile DEPENDS ${_out_urdf_gazebo_file})
+  # generate launch and world file
+  set(ROBOT ${_robot_name})
+  string(TOLOWER ${_robot_name} _robot_sname)
+  if(NOT EXISTS ${PROJECT_SOURCE_DIR}/launch/gazebo_${_robot_sname}_no_controllers.launch)
+    configure_file(${hrpsys_gazebo_general_PACKAGE_PATH}/scripts/default_gazebo_robot_no_controllers.launch.in ${PROJECT_SOURCE_DIR}/launch/gazebo_${_robot_sname}_no_controllers.launch)
+    list(APPEND ${_robot_name}_compile ${PROJECT_SOURCE_DIR}/launch/gazebo_${_robot_sname}_no_controllers.launch)
+  endif()
+  if(NOT EXISTS ${PROJECT_SOURCE_DIR}/launch/${_robot_sname}_hrpsys_bringup.launch)
+    configure_file(${hrpsys_gazebo_general_PACKAGE_PATH}/scripts/default_robot_hrpsys_bringup.launch.in ${_robot_sname}_hrpsys_bringup.launch)
+    list(APPEND ${_robot_name}_compile ${PROJECT_SOURCE_DIR}/launch/${_robot_sname}_hrpsys_bringup.launch)
+  endif()
+  if(NOT EXISTS ${_out_dir}/model.config)
+    configure_file(${hrpsys_gazebo_general_PACKAGE_PATH}/scripts/model.config.in ${_out_dir}/model.config)
+    list(APPEND ${_robot_name}_compile ${_out_dir}/model.config)
+  endif()
+  list(APPEND compile_urdf_robots ${_robot_name}_compile)
+  install(DIRECTORY ${_out_dir} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/robot_models/ PATTERN ".svn" EXCLUDE)
+endmacro()

hrpsys_gazebo_general/config/SampleRobot.yaml

@@ -0,0 +1,122 @@
+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: SampleRobot
+# 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]
+# joints list used in gazebo, sizeof(joint_id_list) == sizeof(joints)
+  joints:
+    - RLEG_HIP_R
+    - RLEG_HIP_P
+    - RLEG_HIP_Y
+    - RLEG_KNEE
+    - RLEG_ANKLE_P
+    - RLEG_ANKLE_R
+    - RARM_SHOULDER_P
+    - RARM_SHOULDER_R
+    - RARM_SHOULDER_Y
+    - RARM_ELBOW
+    - RARM_WRIST_Y
+    - RARM_WRIST_P
+    - RARM_WRIST_R
+    - LLEG_HIP_R
+    - LLEG_HIP_P
+    - LLEG_HIP_Y
+    - LLEG_KNEE
+    - LLEG_ANKLE_P
+    - LLEG_ANKLE_R
+    - LARM_SHOULDER_P
+    - LARM_SHOULDER_R
+    - LARM_SHOULDER_Y
+    - LARM_ELBOW
+    - LARM_WRIST_Y
+    - LARM_WRIST_P
+    - LARM_WRIST_R
+    - WAIST_P
+    - WAIST_R
+    - CHEST
+## comment for joint index
+# 0   - RLEG_HIP_R
+# 1   - RLEG_HIP_P
+# 2   - RLEG_HIP_Y
+# 3   - RLEG_KNEE
+# 4   - RLEG_ANKLE_P
+# 5   - RLEG_ANKLE_R
+# 6   - RARM_SHOULDER_P
+# 7   - RARM_SHOULDER_R
+# 8   - RARM_SHOULDER_Y
+# 9   - RARM_ELBOW
+# 10  - RARM_WRIST_Y
+# 11  - RARM_WRIST_P
+# 12  - RARM_WRIST_R
+# 13  - LLEG_HIP_R
+# 14  - LLEG_HIP_P
+# 15  - LLEG_HIP_Y
+# 16  - LLEG_KNEE
+# 17  - LLEG_ANKLE_P
+# 18  - LLEG_ANKLE_R
+# 19  - LARM_SHOULDER_P
+# 20  - LARM_SHOULDER_R
+# 21  - LARM_SHOULDER_Y
+# 22  - LARM_ELBOW
+# 23  - LARM_WRIST_Y
+# 24  - LARM_WRIST_P
+# 25  - LARM_WRIST_R
+# 26  - WAIST_P
+# 27  - WAIST_R
+# 28  - CHEST
+## joint gain settings
+  gains:
+    LLEG_HIP_R:      {p: 12000.0, d:  4.0, i: 0.0, vp:  6.0, i_clamp: 0.0, p_v: 250.0}
+    LLEG_HIP_P:      {p: 24000.0, d:  6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    LLEG_HIP_Y:      {p:  4000.0, d:  4.0, i: 0.0, vp:  1.0, i_clamp: 0.0, p_v: 250.0}
+    LLEG_KNEE:       {p: 36000.0, d:  6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    LLEG_ANKLE_P:    {p: 18000.0, d:  3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    LLEG_ANKLE_R:    {p:  6000.0, d:  2.0, i: 0.0, vp:  4.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_HIP_R:      {p: 12000.0, d:  4.0, i: 0.0, vp:  6.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_HIP_P:      {p: 24000.0, d:  6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_HIP_Y:      {p:  4000.0, d:  4.0, i: 0.0, vp:  1.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_KNEE:       {p: 36000.0, d:  6.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_ANKLE_P:    {p: 18000.0, d:  3.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    RLEG_ANKLE_R:    {p:  6000.0, d:  2.0, i: 0.0, vp:  4.0, i_clamp: 0.0, p_v: 250.0}
+    WAIST_P:         {p: 8000.0, d:   4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    WAIST_R:         {p: 8000.0, d:   4.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    CHEST:           {p: 6000.0, d:   2.0, i: 0.0, vp: 20.0, i_clamp: 0.0, p_v: 250.0}
+    LARM_SHOULDER_P: {p: 1200.0, d:   1.0, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 160.0}
+    LARM_SHOULDER_R: {p:  500.0, d:   0.5, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 120.0}
+    LARM_SHOULDER_Y: {p:  200.0, d:   0.3, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    LARM_ELBOW:      {p: 1000.0, d:   1.4, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 160.0}
+    LARM_WRIST_Y:    {p:  200.0, d:   0.1, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    LARM_WRIST_P:    {p:  300.0, d:   0.2, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    LARM_WRIST_R:    {p:   20.0, d:   0.1, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    RARM_SHOULDER_P: {p: 1200.0, d:   1.0, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 160.0}
+    RARM_SHOULDER_R: {p:  500.0, d:   0.5, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 120.0}
+    RARM_SHOULDER_Y: {p:  200.0, d:   0.3, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    RARM_ELBOW:      {p: 1000.0, d:   1.4, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 160.0}
+    RARM_WRIST_Y:    {p:  200.0, d:   0.1, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    RARM_WRIST_P:    {p:  300.0, d:   0.2, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+    RARM_WRIST_R:    {p:   20.0, d:   0.1, i: 0.0, vp:  0.0, i_clamp: 0.0, p_v: 100.0}
+## force sensor settings
+## list of force sensorname
+  force_torque_sensors:
+    - lfsensor
+    - rfsensor
+    - lhsensor
+    - rhsensor
+## configuration of force sensor
+## key of force_torque_sensors_config should be a member of force_torque_sensors
+  force_torque_sensors_config:
+    lfsensor: {joint_name: 'LLEG_ANKLE_R', frame_id: 'LLEG_LINK6', translation: [0, 0, 0], rotation: [1, 0, 0, 0]}
+    rfsensor: {joint_name: 'RLEG_ANKLE_R', frame_id: 'RLEG_LINK6', translation: [0, 0, 0], rotation: [1, 0, 0, 0]}
+    lhsensor: {joint_name: 'LARM_WRIST_R', frame_id: 'LARM_LINK7'}
+    rhsensor: {joint_name: 'RARM_WRIST_R', frame_id: 'RARM_LINK7'}
+## IMU sensor settings
+## configuration of IMU sensor
+## key of imu_sensors_config should be a member of imu_sensors
+  imu_sensors:
+    - imu_sensor0
+  imu_sensors_config:
+    imu_sensor0: {ros_name: 'sample_imu_sensor', link_name: 'WAIST_LINK0', frame_id: 'WAIST_LINK0'}

hrpsys_gazebo_general/iob/CMakeLists.txt

@@ -33,9 +33,12 @@ set_target_properties(hrpEC_gazebo PROPERTIES OUTPUT_NAME hrpEC)
 set_target_properties(hrpEC_gazebo PROPERTIES COMPILE_DEFINITIONS "OPENRTM_VERSION110")
 
 
-install(TARGETS RobotHardwareComp_gazebo RobotHardware_gazebo hrpEC_gazebo hrpIo_gazebo
-  RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
-  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+install(TARGETS hrpIo_gazebo
+  LIBRARY DESTINATION lib CONFIGURATIONS Release Debug RelWithDebInfo
+)
+install(TARGETS RobotHardwareComp_gazebo RobotHardware_gazebo hrpEC_gazebo
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} CONFIGURATIONS Release Debug RelWithDebInfo
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/lib CONFIGURATIONS Release Debug RelWithDebInfo
 )
 
 

hrpsys_gazebo_general/launch/gazebo_robot_no_controllers.launch

@@ -0,0 +1,55 @@
+<launch>
+  <arg name="gzname" default="gazebo"/>
+  <arg name="ROBOT_TYPE" default="SampleRobot" />
+  <arg name="WORLD" default="$(find hrpsys_gazebo_general)/worlds/empty.world" />
+  <arg name="HRPSYS_GAZEBO_CONFIG" default="$(find hrpsys_gazebo_general)/config/$(arg ROBOT_TYPE).yaml" />
+  <arg name="ROBOT_MODEL" default="$(find hrpsys_gazebo_general)/robot_models/$(arg ROBOT_TYPE)/$(arg ROBOT_TYPE).urdf.xacro" />
+  <arg name="PAUSED" default="false"/>
+  <arg name="SYNCHRONIZED" default="false" />
+  <arg name="USE_INSTANCE_NAME" default="false" />
+  <arg name="ROBOT_INSTANCE_NAME" default="$(arg ROBOT_TYPE)" />
+
+  <arg name="LOOPBACK" default="false" />
+  <arg name="SPAWN_MODEL" default="false" />
+
+  <arg name="MODEL_TRANSLATE_X" default="0.0" />
+  <arg name="MODEL_TRANSLATE_Y" default="0.0" />
+  <arg name="MODEL_TRANSLATE_Z" default="1.0" />
+
+  <arg if="$(arg PAUSED)"
+       name="paused" value="_paused" />
+  <arg unless="$(arg PAUSED)"
+       name="paused" value="" />
+
+  <group unless="$(arg LOOPBACK)" >
+    <param name="/use_sim_time" type="bool" value="true"/>
+
+    <!-- start gazebo with the hrpsys_gazebo -->
+    <node name="gazebo" pkg="hrpsys_gazebo_general" type="$(arg gzname)" args="$(arg WORLD)" output="screen" />
+  </group>
+
+  <group if="$(arg USE_INSTANCE_NAME)"
+         ns="$(arg ROBOT_INSTANCE_NAME)" >
+    <!-- controller configuration -->
+    <rosparam command="load" file="$(arg HRPSYS_GAZEBO_CONFIG)" />
+    <!-- setting for using synchronized iob -->
+    <param name="hrpsys_gazebo_configuration/use_synchronized_command"
+           value="$(arg SYNCHRONIZED)" />
+  </group>
+
+  <group unless="$(arg USE_INSTANCE_NAME)" >
+    <!-- controller configuration -->
+    <rosparam command="load" file="$(arg HRPSYS_GAZEBO_CONFIG)" />
+    <!-- setting for using synchronized iob -->
+    <param name="hrpsys_gazebo_configuration/use_synchronized_command"
+           value="$(arg SYNCHRONIZED)" />
+  </group>
+
+  <!-- Robot Description -->
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(arg ROBOT_MODEL)'" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+  <!-- Spawn Robot Model -->
+  <node name="spawn_robot_model" pkg="gazebo_ros" type="spawn_model"
+        args="-unpause -urdf -param robot_description -model mobile_base -z 0.73" />
+</launch>

hrpsys_gazebo_general/launch/gazebo_samplerobot_no_controllers.launch

@@ -0,0 +1,18 @@
+<launch>
+  <arg name="gzname" default="gazebo"/>
+  <arg name="WORLD" default="$(find hrpsys_gazebo_general)/worlds/empty.world"/>
+  <arg name="PAUSED" default="false"/>
+  <arg name="SYNCHRONIZED" default="false" />
+
+  <include file="$(find hrpsys_gazebo_general)/launch/gazebo_robot_no_controllers.launch">
+    <arg name="ROBOT_TYPE" value="SampleRobot" />
+    <arg name="WORLD" value="$(arg WORLD)" />
+    <arg name="HRPSYS_GAZEBO_CONFIG" default="$(find hrpsys_gazebo_general)/config/SampleRobot.yaml" />
+    <arg name="ROBOT_MODEL" default="$(find hrpsys_gazebo_general)/robot_models/SampleRobot/SampleRobot.urdf.xacro" />
+
+    <arg name="PAUSED" value="$(arg PAUSED)"/>
+    <arg name="SYNCHRONIZED" value="$(arg SYNCHRONIZED)" />
+    <arg name="USE_INSTANCE_NAME" value="true" />
+    <arg name="gzname" value="$(arg gzname)" />
+  </include>
+</launch>