Skip to content

Commit

Permalink
Merge pull request #133 from mmurooka/move-samplerobot-to-hrpsys-gaze…
Browse files Browse the repository at this point in the history
…b-general

Move samplerobot to hrpsys_gazebo_general
  • Loading branch information
k-okada committed Oct 12, 2014
2 parents 8805518 + ff1dd6f commit e83f8db
Show file tree
Hide file tree
Showing 20 changed files with 580 additions and 15 deletions.
35 changes: 30 additions & 5 deletions hrpsys_gazebo_general/catkin.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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})
53 changes: 53 additions & 0 deletions hrpsys_gazebo_general/cmake/compile_robot_model_for_gazebo.cmake
Original file line number Diff line number Diff line change
@@ -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()
122 changes: 122 additions & 0 deletions hrpsys_gazebo_general/config/SampleRobot.yaml
Original file line number Diff line number Diff line change
@@ -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'}
9 changes: 6 additions & 3 deletions hrpsys_gazebo_general/iob/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)


Expand Down
55 changes: 55 additions & 0 deletions hrpsys_gazebo_general/launch/gazebo_robot_no_controllers.launch
Original file line number Diff line number Diff line change
@@ -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>
Original file line number Diff line number Diff line change
@@ -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>
Loading

0 comments on commit e83f8db

Please sign in to comment.