CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(hrpsys_ros_bridge_tutorials)
#find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge hrpsys openhrp3)
find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge euscollada rostest
  euslisp xacro control_msgs roseus)

find_package(hrp2_models QUIET)
if(NOT "${hrp2_models_FOUND}")
  string(ASCII 27 Esc)
  message(WARNING "${Esc}[1;33mPackage hrp2_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m")
endif()
find_package(jsk_models QUIET)
if(NOT "${jsk_models_FOUND}")
  string(ASCII 27 Esc)
  message(WARNING "${Esc}[1;33mPackage jsk_models is not found, if you have right to access them please include source code in catkin workspace${Esc}[m")
endif()

include(FindPkgConfig)
pkg_check_modules(openhrp3 REQUIRED openhrp3.1)
set(OPENHRP_SAMPLE_DIR ${openhrp3_PREFIX}/share/OpenHRP-3.1/sample)
pkg_check_modules(hrpsys hrpsys-base REQUIRED)
catkin_python_setup()

catkin_package(
    DEPENDS
    CATKIN_DEPENDS hrpsys_ros_bridge euscollada
    INCLUDE_DIRS # TODO include
    LIBRARIES # TODO
)

################################
## compile_openhrp_model
##  Generate OpenHRP3 .xml and .conf file.
##  Convert .wrl, .dae, and .l files
################################
if(EXISTS ${hrpsys_PREFIX}/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl)
  compile_openhrp_model(
    ${hrpsys_PREFIX}/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl
    HRP4C
    -a rightarm_torso,BODY,R_WRIST_R_LINK,0,0,0,0.707,0,0.707,0 -a leftarm_torso,BODY,L_WRIST_R_LINK,0,0,0,0.707,0,0.707,0 -a rightarm,BODY,CHEST_Y_LINK,0,0,0,0.707,0,0.707,0 -a leftarm,CHEST_Y_LINK,L_WRIST_R_LINK,0,0,0,0.707,0,0.707,0
    --conf-file-option "virtual_force_sensor: vlhsensor, CHEST_Y, L_HAND_J0, 0,0,0, 0,0,1,0, vrhsensor, CHEST_Y, R_HAND_J0, 0,0,0, 0,0,1,0"
    --conf-file-option "abc_leg_offset: 0.0, 0.06845, 0.0"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  )
endif(EXISTS ${hrpsys_PREFIX}/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl)

if(EXISTS ${OPENHRP_SAMPLE_DIR}/model/PA10/pa10.main.wrl)
compile_openhrp_model(
  ${OPENHRP_SAMPLE_DIR}/model/PA10/pa10.main.wrl)
endif(EXISTS ${OPENHRP_SAMPLE_DIR}/model/PA10/pa10.main.wrl)
if(EXISTS ${OPENHRP_SAMPLE_DIR}/model/sample1.wrl)
compile_openhrp_model(
  ${OPENHRP_SAMPLE_DIR}/model/sample1.wrl SampleRobot
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --conf-file-option "abc_leg_offset: 0,0.09,0"
  --conf-file-option "abc_stride_parameter: 0.15,0.05,10"
  --conf-file-option "end_effectors: lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, larm,LARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, rarm,RARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708,"
  --conf-file-option "collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R"
  --conf-file-option "pdgains_sim_file_name: ${hrpsys_PREFIX}/share/hrpsys/samples/SampleRobot/SampleRobot.PDgain.dat"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  )
else()
  get_cmake_property(_variableNames VARIABLES)
  foreach (_variableName ${_variableNames})
    message(STATUS "${_variableName}=${${_variableName}}")
  endforeach()
  message(FATAL_ERROR "${OPENHRP_SAMPLE_DIR}/model/sample1.wrl not found")
endif(EXISTS ${OPENHRP_SAMPLE_DIR}/model/sample1.wrl)

# collada_robots
# webots_simulator
# yaskawa
# chreonoid

### convert model for closed models
macro(compile_model_for_closed_robots _robot_wrl_file _OpenHRP2_robot_name)
  if(EXISTS ${_robot_wrl_file})
    compile_openhrp_model(
      ${_robot_wrl_file}
      ${_OpenHRP2_robot_name}
      ${ARGN})
  else()
    message(STATUS "\n\n\n\n ${_robot_wrl_file} is not found..\n\n\n\n")
  endif()
endmacro()
macro(compile_openhrp_model_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2_robot_dir _OpenHRP2_robot_name)
  compile_model_for_closed_robots(
    ${hrp2_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl
    ${_OpenHRP2_robot_name}
    ${ARGN})
endmacro()
macro(compile_rbrain_model_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2_robot_dir _OpenHRP2_robot_name)
  compile_model_for_closed_robots(
    ${jsk_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl
    ${_OpenHRP2_robot_name}
    ${ARGN})
endmacro()
macro(gen_minmax_table_for_closed_robots _OpenHRP2_robot_vrml_name _OpenHRP2_robot_dir _OpenHRP2_robot_name)
  if (EXISTS ${hrp2_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl OR
      EXISTS ${jsk_models_MODEL_DIR}/${_OpenHRP2_robot_dir}/${_OpenHRP2_robot_vrml_name}main.wrl)
    string(TOLOWER ${_OpenHRP2_robot_name} _sname)
    set(_workdir ${PROJECT_SOURCE_DIR}/models)
    set(_gen_jointmm_command_arg "\"\\(write-min-max-table-to-robot-model-file \\(${_sname}\\) \\\"${_workdir}/${_sname}.l\\\" :margin 1.0\\)\"")
    set(_gen_jointmm_conf_command_arg "\"\\(write-min-max-table-to-conf-file \\(${_sname}\\) \\\"${_workdir}/${_OpenHRP2_robot_name}.conf\\\"\\)\"")

    find_program(euslisp_exe irteusgl)
    if(NOT EXISTS ${euslisp_exe})
      message(FATAL_ERROR "failed to find euslisp, skipping generating min max table")
    else()
      message(STATUS "find euslisp on ${euslisp_exe}")
      add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_done
      COMMAND DISPLAY= ${euslisp_exe} ${PROJECT_SOURCE_DIR}/euslisp/make-joint-min-max-table.l ${_workdir}/${_sname}.l "\"${_gen_jointmm_command_arg}\"" "\"(exit)\"" && touch ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_done
      DEPENDS ${_workdir}/${_sname}.l)
      add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_conf_done
      COMMAND DISPLAY= ${euslisp_exe} ${PROJECT_SOURCE_DIR}/euslisp/make-joint-min-max-table.l ${_workdir}/${_sname}.l "\"${_gen_jointmm_conf_command_arg}\"" "\"(exit)\"" && touch ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_conf_done
      DEPENDS ${_workdir}/${_sname}.l ${_workdir}/${_OpenHRP2_robot_name}.xml ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_done)
    add_custom_target(${_sname}_${PROJECT_NAME}_compile_joint_minmax DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_done ${CMAKE_CURRENT_BINARY_DIR}/${_sname}_joint_minmax_conf_done)
    add_dependencies(${_sname}_${PROJECT_NAME}_compile ${_sname}_${PROJECT_NAME}_compile_joint_minmax)
    endif()
  endif()
endmacro()

# old HRP2xx.wrl files should be coverted.
compile_openhrp_model_for_closed_robots(HRP2JSK HRP2JSK_for_OpenHRP3 HRP2JSK
  --conf-file-option "abc_leg_offset: 0.0,0.105,0.0"
  --conf-file-option "# original ankle"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,-0.01,-0.105,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.01,-0.105,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# ankle spacer"
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,-0.01,-0.115,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.01,-0.115,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# for ThermoEstimator"
  --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3"
  --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0"
  --conf-file-option "alarm_ratio: 0.75"
  --conf-file-option "# GraspController"
  --conf-file-option "grasp_joint_groups : LHAND : LARM_JOINT7, 1, RHAND : RARM_JOINT7, -1"
  --conf-file-option "# for Collison Detector"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:RARM_JOINT7 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT2:LARM_JOINT7 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:RARM_JOINT7 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT3:LARM_JOINT7 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:RARM_JOINT7 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT5:LARM_JOINT7 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:RARM_JOINT7 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT2:LARM_JOINT7 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:RARM_JOINT7 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT3:LARM_JOINT7 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:RARM_JOINT7 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT5:LARM_JOINT7 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:RARM_JOINT7 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 CHEST_JOINT1:LARM_JOINT7 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:RARM_JOINT7 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 HEAD_JOINT1:LARM_JOINT7 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:LARM_JOINT7 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:LARM_JOINT7 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:LARM_JOINT7 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:LARM_JOINT7 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:LARM_JOINT7 RARM_JOINT6:WAIST RARM_JOINT7:LARM_JOINT0 RARM_JOINT7:LARM_JOINT2 RARM_JOINT7:LARM_JOINT3 RARM_JOINT7:LARM_JOINT4 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:LARM_JOINT6 RARM_JOINT7:LARM_JOINT7 RARM_JOINT7:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST LARM_JOINT7:WAIST"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# for PDcontroller"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSK.PDgain.dat"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-dt-option "0.004"
  --simulation-timestep-option "0.004"
  )

gen_minmax_table_for_closed_robots(HRP2JSK HRP2JSK_for_OpenHRP3 HRP2JSK)

compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_for_OpenHRP3 HRP2JSKNT
  --conf-file-option "abc_leg_offset: 0.0,0.105,0.0"
  --conf-file-option "# 6dof joint version"
  --conf-file-option "#end_effectors: rleg,RLEG_JOINT5,WAIST,0.035589,-0.01,-0.105,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.035589,0.01,-0.105,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# Original foot + 7dof joint version"
  --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# Karasawa sole plate + 7dof joint version"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.034,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.034,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# for ThermoEstimator"
  --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3"
  --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0"
  --conf-file-option "alarm_ratio: 0.75"
  --conf-file-option "# for CollisionDetector"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT6 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT6 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT6 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT6:LLEG_JOINT2 RLEG_JOINT6:LLEG_JOINT3 RLEG_JOINT6:LLEG_JOINT5 RLEG_JOINT6:LLEG_JOINT6 RLEG_JOINT6:CHEST_JOINT1 RLEG_JOINT6:RARM_JOINT3 RLEG_JOINT6:RARM_JOINT4 RLEG_JOINT6:RARM_JOINT5 RLEG_JOINT6:RARM_JOINT6 RLEG_JOINT6:LARM_JOINT3 RLEG_JOINT6:LARM_JOINT4 RLEG_JOINT6:LARM_JOINT5 RLEG_JOINT6:LARM_JOINT6 RLEG_JOINT6:WAIST LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT6:CHEST_JOINT1 LLEG_JOINT6:RARM_JOINT3 LLEG_JOINT6:RARM_JOINT4 LLEG_JOINT6:RARM_JOINT5 LLEG_JOINT6:RARM_JOINT6 LLEG_JOINT6:LARM_JOINT3 LLEG_JOINT6:LARM_JOINT4 LLEG_JOINT6:LARM_JOINT5 LLEG_JOINT6:LARM_JOINT6 LLEG_JOINT6:WAIST CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# for PDcontroller"
  --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNT.PDgain.dat"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-dt-option "0.004"
  --simulation-timestep-option "0.004"
  )
gen_minmax_table_for_closed_robots(HRP2JSKNT HRP2JSKNT_for_OpenHRP3 HRP2JSKNT)
compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS
  --conf-file-option "abc_leg_offset: 0.0,0.105,0.0"
  --conf-file-option "# 6dof joint version"
  --conf-file-option "#end_effectors: rleg,RLEG_JOINT5,WAIST,0.035589,-0.01,-0.105,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.035589,0.01,-0.105,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# Original foot + 7dof joint version"
  --conf-file-option "#end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.031,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.031,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# Karasawa sole plate + 7dof joint version"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT6,WAIST,-0.079411,-0.01,-0.034,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT6,WAIST,-0.079411,0.01,-0.034,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT1,-0.0042,0.0392,-0.1245,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,-0.0042,-0.0392,-0.1245,0.0,1.0,0.0,1.5708,"
  --conf-file-option "# for ThermoEstimator"
  --conf-file-option "motor_heat_params: 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.0075,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.003,0.5, 0.0075,0.5, 0.02,0.3, 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3"
  --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 45.0, 60.0, 40.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0"
  --conf-file-option "alarm_ratio: 0.75"
  --conf-file-option "# for CollisionDetector"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT6 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT6 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT6 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 RLEG_JOINT6:LLEG_JOINT2 RLEG_JOINT6:LLEG_JOINT3 RLEG_JOINT6:LLEG_JOINT5 RLEG_JOINT6:LLEG_JOINT6 RLEG_JOINT6:CHEST_JOINT1 RLEG_JOINT6:RARM_JOINT3 RLEG_JOINT6:RARM_JOINT4 RLEG_JOINT6:RARM_JOINT5 RLEG_JOINT6:RARM_JOINT6 RLEG_JOINT6:LARM_JOINT3 RLEG_JOINT6:LARM_JOINT4 RLEG_JOINT6:LARM_JOINT5 RLEG_JOINT6:LARM_JOINT6 RLEG_JOINT6:WAIST LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 LLEG_JOINT6:CHEST_JOINT1 LLEG_JOINT6:RARM_JOINT3 LLEG_JOINT6:RARM_JOINT4 LLEG_JOINT6:RARM_JOINT5 LLEG_JOINT6:RARM_JOINT6 LLEG_JOINT6:LARM_JOINT3 LLEG_JOINT6:LARM_JOINT4 LLEG_JOINT6:LARM_JOINT5 LLEG_JOINT6:LARM_JOINT6 LLEG_JOINT6:WAIST CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# for PDcontroller"
  --conf-file-option "pdcontrol_tlimit_ratio:100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100,100"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-dt-option "0.004"
  --simulation-timestep-option "0.004"
  )
gen_minmax_table_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS)
compile_openhrp_model_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W
  --conf-file-option "end_effectors: rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708"
  --conf-file-option "# for ThermoEstimator"
  --conf-file-option "motor_heat_params: 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3"
  --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0"
  --conf-file-option "alarm_ratio: 0.75"
  --conf-file-option "# for collision detector"
  --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# GraspController"
  --conf-file-option "grasp_joint_groups : LHAND : LARM_JOINT7, 1, RHAND : RARM_JOINT7, -1"
  --conf-file-option "# for CollisionDetector"
  --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat"
  --conf-dt-option "0.004"
  --simulation-timestep-option "0.004"
  --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708"
  )
gen_minmax_table_for_closed_robots(HRP2W HRP2W_for_OpenHRP3 HRP2W)
compile_openhrp_model_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G
  --conf-file-option "end_effectors: rarm,RARM_JOINT6,CHEST_JOINT1,0.0,0.0169,-0.174,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT1,0.0,-0.0169,-0.174,0.0,1.0,0.0,1.5708"
  --conf-file-option "# for ThermoEstimator"
  --conf-file-option "motor_heat_params: 0.003,0.5, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.003,0.5, 0.003,0.5, 0.02,0.3, 0.003,0.5, 0.2,0.3, 0.2,0.3, 0.2,0.3, 0.2,0.3"
  --conf-file-option "motor_temperature_limit: 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0"
  --conf-file-option "alarm_ratio: 0.75"
  --conf-file-option "# for collision detector"
  --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# GraspController"
  --conf-file-option "grasp_joint_groups : LHAND : LARM_JOINT7, 1, RHAND : RARM_JOINT7, -1"
  --conf-file-option "# for CollisionDetector"
  --conf-file-option "collision_pair: CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST"
  --conf-file-option "collision_model: convex hull"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/HRP2JSKNTS.PDgain.dat"
  --conf-dt-option "0.004"
  --simulation-timestep-option "0.004"
  --simulation-joint-properties-option "RARM_JOINT3.angle,-1.5708,LARM_JOINT3.angle,-1.5708"
  )
gen_minmax_table_for_closed_robots(HRP2G HRP2G_for_OpenHRP3 HRP2G)
# compile_openhrp_model_for_closed_robots(HRP2JSKNT HRP2JSKNT_WITH_3HAND HRP2JSKNT_WITH_3HAND
#  -a leftarm,CHEST_LINK1,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a leftarm_torso,BODY,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a leftarm_grasp,CHEST_LINK1,LARM_LINK6,0.0,-0.03,-0.17,1.0,0.0,0.0,2.0944,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a rightarm,CHEST_LINK1,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#  -a rightarm_torso,BODY,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#  -a rightarm_grasp,CHEST_LINK1,RARM_LINK6,0.0,0.03,-0.17,-1.0,0.0,0.0,2.0944,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#   )
# compile_openhrp_model_for_closed_robots(HRP2JSKNTS HRP2JSKNTS_WITH_3HAND HRP2JSKNTS_WITH_3HAND
#   -a leftarm,CHEST_LINK1,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a leftarm_torso,BODY,LARM_LINK6,-0.0042,-0.0392,-0.1245,-3.373247e-18,1.0,9.813081e-18,1.5708,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a leftarm_grasp,CHEST_LINK1,LARM_LINK6,0.0,-0.03,-0.17,1.0,0.0,0.0,2.0944,L_THUMBCM_Y,0,L_THUMBCM_P,1,L_INDEXMP_R,0,L_INDEXMP_P,0,L_INDEXPIP_R,-1,L_MIDDLEPIP_R,-1
#  -a rightarm,CHEST_LINK1,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#  -a rightarm_torso,BODY,RARM_LINK6,-0.0042,0.0392,-0.1245,3.373247e-18,1.0,-9.813081e-18,1.5708,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#  -a rightarm_grasp,CHEST_LINK1,RARM_LINK6,0.0,0.03,-0.17,-1.0,0.0,0.0,2.0944,R_THUMBCM_Y,0,R_THUMBCM_P,1,R_INDEXMP_R,0,R_INDEXMP_P,0,R_INDEXPIP_R,1,R_MIDDLEPIP_R,1
#   )
compile_openhrp_model_for_closed_robots(HRP4R HRP4R HRP4R
  -a leftarm,L_SHOULDER_P_LINK,L_WRIST_R_LINK,0,0,0,0,0,0,1,L_HAND_J0,-1,L_HAND_J1,-1
  -a rightarm,R_SHOULDER_P_LINK,R_WRIST_R_LINK,0,0,0,0,0,0,1,R_HAND_J0,1,R_HAND_J1,1
  --conf-file-option "virtual_force_sensor: vlhsensor, CHEST_Y, L_WRIST_R, 0,0,0, 0,0,1,0, vrhsensor, CHEST_Y, R_WRIST_R, 0,0,0, 0,0,1,0, vlfsensor, WAIST, L_ANKLE_R, 0,0,0, 0,0,1,0, vrfsensor, WAIST, R_ANKLE_R, 0,0,0, 0,0,1,0"
  --conf-file-option "abc_leg_offset: 0.0, 0.079919, 0.0"
  --conf-file-option "end_effectors: rarm,R_WRIST_R,CHEST_Y,0.0,0.0,-0.1,-1.471962e-17,1.0,-1.471962e-17,1.5708, larm,L_WRIST_R,CHEST_Y,0.0,0.0,-0.1,1.471962e-17,1.0,1.471962e-17,1.5708, rleg,R_ANKLE_R,WAIST,0.0,0.0,-0.091849,0.0,0.0,0.0,0.0, lleg,L_ANKLE_R,WAIST,0.0,0.0,-0.091849,0.0,0.0,0.0,0.0,"
  )

if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_L/HRP3HAND_Lmain.wrl)
  compile_openhrp_model(
    ${hrp2_models_MODEL_DIR}/HRP3HAND_L/HRP3HAND_Lmain.wrl
    HRP3HAND_L)
endif()

if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl)
  compile_openhrp_model(
    ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl
    HRP3HAND_R)
endif()

# HIRONXJSK
compile_openhrp_model_for_closed_robots(HIRONXJSK HIRONXJSK HIRONXJSK
  --conf-dt-option "0.005"
  --conf-file-option "# This conf is only for hrpsys simulation and different from /opt/jsk/etc/HIRONX/hrprtc/Robot.conf inside HIRONXJSK"
  --conf-file-option "# RTC disable/enable settings which will be loaded via RobotHardware"
  --conf-file-option "CollisionDetector.enable: YES"
  --conf-file-option "# for ImpedanceController"
  --conf-file-option "end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708, larm,LARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708,"
  --conf-file-option "# for CollisionDetector"
  --conf-file-option "collision_pair: HEAD_JOINT0:RARM_JOINT2 HEAD_JOINT0:RARM_JOINT3 HEAD_JOINT0:RARM_JOINT4 HEAD_JOINT0:RARM_JOINT5 HEAD_JOINT0:LARM_JOINT2 HEAD_JOINT0:LARM_JOINT3 HEAD_JOINT0:LARM_JOINT4 HEAD_JOINT0:LARM_JOINT5 HEAD_JOINT1:RARM_JOINT2 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:LARM_JOINT2 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 WAIST_JOINT:RARM_JOINT3 WAIST_JOINT:RARM_JOINT4 WAIST_JOINT:RARM_JOINT5 WAIST_JOINT:LARM_JOINT3 WAIST_JOINT:LARM_JOINT4 WAIST_JOINT:LARM_JOINT5 BASE_TOP_JOINT:RARM_JOINT3 BASE_TOP_JOINT:RARM_JOINT4 BASE_TOP_JOINT:RARM_JOINT5 BASE_TOP_JOINT:LARM_JOINT3 BASE_TOP_JOINT:LARM_JOINT4 BASE_TOP_JOINT:LARM_JOINT5 BASE_BOTTOM_JOINT:RARM_JOINT3 BASE_BOTTOM_JOINT:RARM_JOINT4 BASE_BOTTOM_JOINT:RARM_JOINT5 BASE_BOTTOM_JOINT:LARM_JOINT3 BASE_BOTTOM_JOINT:LARM_JOINT4 BASE_BOTTOM_JOINT:LARM_JOINT5 CHEST_JOINT0:RARM_JOINT2 CHEST_JOINT0:RARM_JOINT3 CHEST_JOINT0:RARM_JOINT4 CHEST_JOINT0:RARM_JOINT5 CHEST_JOINT0:LARM_JOINT2 CHEST_JOINT0:LARM_JOINT3 CHEST_JOINT0:LARM_JOINT4 CHEST_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT1 RARM_JOINT0:LARM_JOINT2 RARM_JOINT0:LARM_JOINT3 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT1:LARM_JOINT0 RARM_JOINT1:LARM_JOINT1 RARM_JOINT1:LARM_JOINT2 RARM_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT2:LARM_JOINT0 RARM_JOINT2:LARM_JOINT1 RARM_JOINT2:LARM_JOINT2 RARM_JOINT2:LARM_JOINT3 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT3:LARM_JOINT0 RARM_JOINT3:LARM_JOINT1 RARM_JOINT3:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5"
  )

# URATALEG
compile_rbrain_model_for_closed_robots(URATALEG URATALEG URATALEG
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.08, 0.0"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0,"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --conf-file-option "collision_loop: 10"
#  --conf-file-option "collision_pair: WAIST:LLEG_JOINT1 WAIST:LLEG_JOINT2 WAIST:LLEG_JOINT3 WAIST:LLEG_JOINT4 WAIST:LLEG_JOINT5 WAIST:RLEG_JOINT1 WAIST:RLEG_JOINT2 WAIST:RLEG_JOINT3 WAIST:RLEG_JOINT4 WAIST:RLEG_JOINT5 LLEG_JOINT0:RLEG_JOINT0 LLEG_JOINT0:LLEG_JOINT2 LLEG_JOINT0:LLEG_JOINT3 LLEG_JOINT0:LLEG_JOINT4 LLEG_JOINT0:LLEG_JOINT5 LLEG_JOINT0:RLEG_JOINT1 LLEG_JOINT0:RLEG_JOINT2 LLEG_JOINT0:RLEG_JOINT3 LLEG_JOINT0:RLEG_JOINT4 LLEG_JOINT0:RLEG_JOINT5 RLEG_JOINT0:LLEG_JOINT1 RLEG_JOINT0:LLEG_JOINT2 RLEG_JOINT0:LLEG_JOINT3 RLEG_JOINT0:LLEG_JOINT4 RLEG_JOINT0:LLEG_JOINT5 RLEG_JOINT0:RLEG_JOINT2 RLEG_JOINT0:RLEG_JOINT3 RLEG_JOINT0:RLEG_JOINT4 RLEG_JOINT0:RLEG_JOINT5 LLEG_JOINT1:LLEG_JOINT3 LLEG_JOINT1:LLEG_JOINT4 LLEG_JOINT1:LLEG_JOINT5 LLEG_JOINT1:RLEG_JOINT1 LLEG_JOINT1:RLEG_JOINT2 LLEG_JOINT1:RLEG_JOINT3 LLEG_JOINT1:RLEG_JOINT4 LLEG_JOINT1:RLEG_JOINT5 LLEG_JOINT2:LLEG_JOINT4 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT1 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT4 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:LLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT1 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT4 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT4:RLEG_JOINT1 LLEG_JOINT4:RLEG_JOINT2 LLEG_JOINT4:RLEG_JOINT3 LLEG_JOINT4:RLEG_JOINT4 LLEG_JOINT4:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT1 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT4 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT1:RLEG_JOINT3 RLEG_JOINT1:RLEG_JOINT4 RLEG_JOINT1:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT4 RLEG_JOINT2:RLEG_JOINT5 RLEG_JOINT3:RLEG_JOINT5"
  --conf-file-option "collision_pair: WAIST:LLEG_JOINT1 WAIST:LLEG_JOINT3 WAIST:LLEG_JOINT4 WAIST:RLEG_JOINT1 WAIST:RLEG_JOINT2 WAIST:RLEG_JOINT3 WAIST:RLEG_JOINT4 WAIST:RLEG_JOINT5 LLEG_JOINT0:RLEG_JOINT0 LLEG_JOINT0:LLEG_JOINT3 LLEG_JOINT0:LLEG_JOINT4 LLEG_JOINT0:LLEG_JOINT5 LLEG_JOINT0:RLEG_JOINT1 LLEG_JOINT0:RLEG_JOINT2 LLEG_JOINT0:RLEG_JOINT3 LLEG_JOINT0:RLEG_JOINT4 LLEG_JOINT0:RLEG_JOINT5 RLEG_JOINT0:LLEG_JOINT1 RLEG_JOINT0:LLEG_JOINT2 RLEG_JOINT0:LLEG_JOINT3 RLEG_JOINT0:LLEG_JOINT4 RLEG_JOINT0:LLEG_JOINT5 RLEG_JOINT0:RLEG_JOINT3 RLEG_JOINT0:RLEG_JOINT4 RLEG_JOINT0:RLEG_JOINT5 LLEG_JOINT1:LLEG_JOINT3 LLEG_JOINT1:LLEG_JOINT4 LLEG_JOINT1:LLEG_JOINT5 LLEG_JOINT1:RLEG_JOINT1 LLEG_JOINT1:RLEG_JOINT2 LLEG_JOINT1:RLEG_JOINT3 LLEG_JOINT1:RLEG_JOINT4 LLEG_JOINT1:RLEG_JOINT5 LLEG_JOINT2:LLEG_JOINT4 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT1 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT4 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT1 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT4 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT4:RLEG_JOINT1 LLEG_JOINT4:RLEG_JOINT2 LLEG_JOINT4:RLEG_JOINT3 LLEG_JOINT4:RLEG_JOINT4 LLEG_JOINT4:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT1 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT4 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT1:RLEG_JOINT3 RLEG_JOINT1:RLEG_JOINT4 RLEG_JOINT1:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT4 RLEG_JOINT2:RLEG_JOINT5"
  --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)"
  --conf-file-option "seq_optional_data_dim: 4"
  )

# STARO
compile_rbrain_model_for_closed_robots(STARO STARO STARO
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  --conf-file-option "## KAWADA FOOT"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT1,-2.842171e-17,-0.1893,0.0,0.678598,-0.678598,-0.281085,2.59356, larm,LARM_JOINT7,CHEST_JOINT1,-5.684342e-17,0.1893,0.0,-0.678598,-0.678598,0.281085,2.59356,"
  --conf-file-option "## JSK FOOT"
  --conf-file-option "#end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT1,-2.842171e-17,-0.1893,0.0,0.678598,-0.678598,-0.281085,2.59356, larm,LARM_JOINT7,CHEST_JOINT1,-5.684342e-17,0.1893,0.0,-0.678598,-0.678598,0.281085,2.59356,"
  --conf-file-option "torque_offset: 0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,   0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0"
  --conf-file-option "torque_filter_params: 2, 1.0, 1.88903, -0.89487, 0.0014603, 0.0029206, 0.0014603"
  --conf-file-option "error_to_torque_gain: 0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,   0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0"
  --conf-file-option "error_dead_zone: 0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0,   0, 0, 0, 0, 0, 0,  0, 0, 0, 0, 0, 0"
  --conf-file-option "torque_gain: 0.0001"
  --conf-file-option "collision_loop: 20"
  --conf-file-option "collision_pair: WAIST:RARM_JOINT2 WAIST:LARM_JOINT2 RLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT2:LLEG_JOINT5 RARM_JOINT7:LARM_JOINT7 RARM_JOINT6:LARM_JOINT7 RARM_JOINT7:LARM_JOINT6 LARM_JOINT7:RLEG_JOINT5 RARM_JOINT7:RLEG_JOINT5 RARM_JOINT5:LARM_JOINT7 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:LLEG_JOINT5 LARM_JOINT7:LLEG_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT7:RLEG_JOINT4 LARM_JOINT7:RLEG_JOINT4 RARM_JOINT7:LLEG_JOINT4 LARM_JOINT6:LLEG_JOINT5 RARM_JOINT6:LARM_JOINT5 LARM_JOINT6:RLEG_JOINT5 RARM_JOINT4:LARM_JOINT7 RARM_JOINT7:LARM_JOINT4 RARM_JOINT6:LLEG_JOINT5 LARM_JOINT7:LLEG_JOINT4 RARM_JOINT5:LARM_JOINT6 RARM_JOINT6:RLEG_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT7:RLEG_JOINT3 LARM_JOINT7:LLEG_JOINT3 RARM_JOINT7:LLEG_JOINT3 RARM_JOINT7:LARM_JOINT3 RARM_JOINT5:LARM_JOINT5 LARM_JOINT7:RLEG_JOINT3 RARM_JOINT6:LLEG_JOINT4 LARM_JOINT6:LLEG_JOINT4 RARM_JOINT6:RLEG_JOINT4 RLEG_JOINT5:LLEG_JOINT5 LARM_JOINT5:RLEG_JOINT5 RARM_JOINT5:RLEG_JOINT5 RARM_JOINT5:LLEG_JOINT5 RARM_JOINT3:LARM_JOINT7 LARM_JOINT5:LLEG_JOINT5 RARM_JOINT6:LARM_JOINT4 LARM_JOINT6:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT4 LARM_JOINT6:RLEG_JOINT3 LARM_JOINT5:RLEG_JOINT4 RARM_JOINT6:LLEG_JOINT3 RARM_JOINT5:RLEG_JOINT4 RARM_JOINT2:LARM_JOINT7 RARM_JOINT5:LARM_JOINT4 LARM_JOINT4:LLEG_JOINT5 RARM_JOINT6:RLEG_JOINT3 RARM_JOINT7:LARM_JOINT2 LARM_JOINT4:RLEG_JOINT5 RARM_JOINT5:LLEG_JOINT4 RLEG_JOINT4:LLEG_JOINT5 RARM_JOINT4:RLEG_JOINT5 RARM_JOINT7:RLEG_JOINT2 RARM_JOINT6:LARM_JOINT3 LARM_JOINT7:RLEG_JOINT2 LARM_JOINT5:LLEG_JOINT4 RARM_JOINT4:LLEG_JOINT5 RARM_JOINT4:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 LARM_JOINT7:LLEG_JOINT2 RARM_JOINT7:LLEG_JOINT2 RARM_JOINT3:LLEG_JOINT5 RARM_JOINT3:LARM_JOINT5 RARM_JOINT6:RLEG_JOINT2 RARM_JOINT5:LLEG_JOINT3 RARM_JOINT3:RLEG_JOINT5 RARM_JOINT5:RLEG_JOINT3 RARM_JOINT6:LLEG_JOINT2 LARM_JOINT5:RLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT3 LARM_JOINT4:LLEG_JOINT4 LARM_JOINT4:RLEG_JOINT4 RLEG_JOINT4:LLEG_JOINT4 LARM_JOINT3:LLEG_JOINT5 LARM_JOINT5:LLEG_JOINT3 LARM_JOINT3:RLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT5 RARM_JOINT5:LARM_JOINT3 LARM_JOINT6:RLEG_JOINT2 LARM_JOINT6:LLEG_JOINT2 RARM_JOINT1:LARM_JOINT7 RARM_JOINT7:LARM_JOINT1 RARM_JOINT5:RLEG_JOINT2 LARM_JOINT5:LLEG_JOINT2 RARM_JOINT4:LLEG_JOINT3 LARM_JOINT5:RLEG_JOINT2 LARM_JOINT4:LLEG_JOINT3 LARM_JOINT4:RLEG_JOINT3 LARM_JOINT3:LLEG_JOINT4 LARM_JOINT3:RLEG_JOINT4 LARM_JOINT2:LLEG_JOINT5 LARM_JOINT2:RLEG_JOINT5 RARM_JOINT5:LLEG_JOINT2 LARM_JOINT7:RLEG_JOINT0 RARM_JOINT7:LLEG_JOINT0 RARM_JOINT7:RLEG_JOINT0 RARM_JOINT2:RLEG_JOINT5 RARM_JOINT2:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT4 RARM_JOINT3:RLEG_JOINT4 LARM_JOINT7:LLEG_JOINT0 RARM_JOINT3:LLEG_JOINT4 RLEG_JOINT4:LLEG_JOINT3 RARM_JOINT4:RLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT2 RARM_JOINT1:LLEG_JOINT5 WAIST:RARM_JOINT7 RLEG_JOINT1:LLEG_JOINT5 RARM_JOINT1:RLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT4 LARM_JOINT1:RLEG_JOINT5 RLEG_JOINT5:LLEG_JOINT1 LARM_JOINT6:LLEG_JOINT0 LARM_JOINT1:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT3 LARM_JOINT4:LLEG_JOINT2 WAIST:LARM_JOINT7 RARM_JOINT4:LLEG_JOINT2 LARM_JOINT4:RLEG_JOINT2 LARM_JOINT3:RLEG_JOINT3 RLEG_JOINT4:LLEG_JOINT2 LARM_JOINT3:LLEG_JOINT3 RARM_JOINT6:RLEG_JOINT0 RARM_JOINT4:RLEG_JOINT2 RARM_JOINT3:RLEG_JOINT3 RARM_JOINT2:RLEG_JOINT4 RARM_JOINT6:LLEG_JOINT0 RARM_JOINT3:LLEG_JOINT3 RARM_JOINT0:RARM_JOINT7 LARM_JOINT2:RLEG_JOINT3 LARM_JOINT0:LLEG_JOINT5 RLEG_JOINT3:LLEG_JOINT2 LARM_JOINT3:RLEG_JOINT2 RARM_JOINT0:RLEG_JOINT5 RARM_JOINT5:RLEG_JOINT0 LARM_JOINT3:LLEG_JOINT2 LARM_JOINT2:LLEG_JOINT3 RARM_JOINT3:RLEG_JOINT2 RARM_JOINT3:LLEG_JOINT2 RARM_JOINT5:LLEG_JOINT0 RLEG_JOINT4:LLEG_JOINT1 RLEG_JOINT0:LLEG_JOINT5 WAIST:RARM_JOINT6 LARM_JOINT5:RLEG_JOINT0 RARM_JOINT1:RLEG_JOINT4 WAIST:LARM_JOINT6 RLEG_JOINT2:LLEG_JOINT3 LARM_JOINT0:LARM_JOINT7 RARM_JOINT2:RLEG_JOINT3 LARM_JOINT5:LLEG_JOINT0 RLEG_JOINT5:LLEG_JOINT0 RARM_JOINT0:RARM_JOINT6 WAIST:RARM_JOINT5 WAIST:RLEG_JOINT5 WAIST:LARM_JOINT5 WAIST:LLEG_JOINT5 LARM_JOINT4:LLEG_JOINT0 RARM_JOINT4:RLEG_JOINT0 RLEG_JOINT0:LLEG_JOINT4 RLEG_JOINT1:LLEG_JOINT3 RARM_JOINT1:RLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT2 LARM_JOINT0:LARM_JOINT6 RARM_JOINT1:RARM_JOINT7 RLEG_JOINT4:LLEG_JOINT0 LARM_JOINT2:LLEG_JOINT2 LARM_JOINT2:RLEG_JOINT2 RARM_JOINT0:RLEG_JOINT4 LARM_JOINT1:LLEG_JOINT3 LARM_JOINT1:RLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT1 LARM_JOINT1:LARM_JOINT7 RLEG_JOINT2:LLEG_JOINT1 RLEG_JOINT1:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT0 RLEG_JOINT0:LLEG_JOINT3 RLEG_JOINT0:RLEG_JOINT5 LLEG_JOINT0:LLEG_JOINT5 RARM_JOINT0:RLEG_JOINT3 WAIST:LLEG_JOINT4 LARM_JOINT0:LARM_JOINT5 RARM_JOINT3:RLEG_JOINT0 RARM_JOINT0:RARM_JOINT5 LARM_JOINT3:LLEG_JOINT0 RARM_JOINT0:LLEG_JOINT3 RARM_JOINT2:RARM_JOINT7 RARM_JOINT1:RARM_JOINT6 RARM_JOINT1:RLEG_JOINT2 LARM_JOINT1:LLEG_JOINT2 WAIST:RARM_JOINT4 LARM_JOINT2:LARM_JOINT7 LARM_JOINT0:RLEG_JOINT3 WAIST:LARM_JOINT4 LARM_JOINT1:RLEG_JOINT2 LARM_JOINT1:LARM_JOINT6 WAIST:RLEG_JOINT4 LARM_JOINT0:LLEG_JOINT3 WAIST:RLEG_JOINT3 RARM_JOINT0:RLEG_JOINT2 WAIST:LARM_JOINT3 RLEG_JOINT1:RLEG_JOINT5 RARM_JOINT1:RARM_JOINT5 WAIST:RARM_JOINT3 WAIST:LLEG_JOINT3 LARM_JOINT3:LARM_JOINT7 LARM_JOINT1:LARM_JOINT5 LARM_JOINT0:LLEG_JOINT2 LARM_JOINT0:RLEG_JOINT2 RARM_JOINT3:RARM_JOINT7 RLEG_JOINT0:LLEG_JOINT2 LLEG_JOINT1:LLEG_JOINT5 RLEG_JOINT2:LLEG_JOINT0 WAIST:RARM_JOINT2 LLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5 WAIST:LARM_JOINT2"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  )
gen_minmax_table_for_closed_robots(STARO STARO STARO)

# YSTLEG
compile_rbrain_model_for_closed_robots(YSTLEG YSTLEG YSTLEG
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.08, 0.0"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0,"
  --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)"
  --conf-file-option "seq_optional_data_dim: 4"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  )


# JAXON
compile_rbrain_model_for_closed_robots(JAXON JAXON JAXON
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  ## Leptrino foot
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.11,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708,"
  ## KAWADA foot
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708,"
  ## KAWADA foot + hand contact end-coords
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot + hand contact end-coords"
  ## KAWADA foot with rubber shoes
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.100,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708,"
  ## JSK foot
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708,"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST RLEG_JOINT2:LARM_JOINT7 RLEG_JOINT3:LARM_JOINT7 RLEG_JOINT5:LARM_JOINT7 LLEG_JOINT2:LARM_JOINT7 LLEG_JOINT3:LARM_JOINT7 LLEG_JOINT5:LARM_JOINT7 CHEST_JOINT1:LARM_JOINT7 HEAD_JOINT1:LARM_JOINT7 RARM_JOINT0:LARM_JOINT7 RARM_JOINT2:LARM_JOINT7 RARM_JOINT3:LARM_JOINT7 RARM_JOINT4:LARM_JOINT7 RARM_JOINT5:LARM_JOINT7 RARM_JOINT7:LARM_JOINT7 LARM_JOINT7:WAIST RLEG_JOINT2:RARM_JOINT7 RLEG_JOINT3:RARM_JOINT7 RLEG_JOINT5:RARM_JOINT7 LLEG_JOINT2:RARM_JOINT7 LLEG_JOINT3:RARM_JOINT7 LLEG_JOINT5:RARM_JOINT7 CHEST_JOINT1:RARM_JOINT7 HEAD_JOINT1:RARM_JOINT7 RARM_JOINT7:LARM_JOINT0 RARM_JOINT7:LARM_JOINT2 RARM_JOINT7:LARM_JOINT3 RARM_JOINT7:LARM_JOINT4 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:RARM_JOINT7 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT7 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --simulation-joint-properties-option "RARM_JOINT2.angle,-0.191986,LARM_JOINT2.angle,0.191986"
)
gen_minmax_table_for_closed_robots(JAXON JAXON JAXON)

if("$ENV{TRAVIS_JOB_ID}" STREQUAL "")
# JAXON_RED
compile_rbrain_model_for_closed_robots(JAXON_RED JAXON_RED JAXON_RED
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  ## Leptrino force sensor foot with sponge sole (10mm)
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1155,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1155,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0,1.0,0.0,1.5708,"
  ## KAWADA foot
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708,"
  ## KAWADA foot with rubber shoes
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708,"
  ## KAWADA foot with rubber shoes + hand contact end-coords
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot with rubber shoes + hand contact end-coords"
  ## JSK foot : mechanical 103.5mm, sole rubber 2mm
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, rarm,RARM_JOINT7,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT7,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708,"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST RLEG_JOINT2:LARM_JOINT7 RLEG_JOINT3:LARM_JOINT7 RLEG_JOINT5:LARM_JOINT7 LLEG_JOINT2:LARM_JOINT7 LLEG_JOINT3:LARM_JOINT7 LLEG_JOINT5:LARM_JOINT7 CHEST_JOINT1:LARM_JOINT7 HEAD_JOINT1:LARM_JOINT7 RARM_JOINT0:LARM_JOINT7 RARM_JOINT2:LARM_JOINT7 RARM_JOINT3:LARM_JOINT7 RARM_JOINT4:LARM_JOINT7 RARM_JOINT5:LARM_JOINT7 RARM_JOINT7:LARM_JOINT7 LARM_JOINT7:WAIST RLEG_JOINT2:RARM_JOINT7 RLEG_JOINT3:RARM_JOINT7 RLEG_JOINT5:RARM_JOINT7 LLEG_JOINT2:RARM_JOINT7 LLEG_JOINT3:RARM_JOINT7 LLEG_JOINT5:RARM_JOINT7 CHEST_JOINT1:RARM_JOINT7 HEAD_JOINT1:RARM_JOINT7 RARM_JOINT7:LARM_JOINT0 RARM_JOINT7:LARM_JOINT2 RARM_JOINT7:LARM_JOINT3 RARM_JOINT7:LARM_JOINT4 RARM_JOINT7:LARM_JOINT5 RARM_JOINT7:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:RARM_JOINT7 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT7 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --simulation-joint-properties-option "RARM_JOINT2.angle,-0.191986,LARM_JOINT2.angle,0.191986"
)
gen_minmax_table_for_closed_robots(JAXON_RED JAXON_RED JAXON_RED)
endif()

# JAXON_BLUE
compile_rbrain_model_for_closed_robots(JAXON_BLUE JAXON_BLUE JAXON_BLUE
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/JAXON_BLUE.PDgains_sim.dat"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  ## KAWADA foot
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.096,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.055,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,-0.055,-0.217,0,1.0,0.0,1.5708,"
  ## KAWADA foot with rubber shoes
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.220,0.0,1.0,0.0,1.5708,"
  ## KAWADA foot with rubber shoes + hand contact end-coords
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,-1.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0.0,-0.238,0.0,0.0,1.0,1.5708,# KAWADA foot with rubber shoes + hand contact end-coords"
  ## JSK foot : mechanical 103.5mm, sole rubber 2mm
  --conf-file-option "# end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1055,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708, larm,LARM_JOINT6,CHEST_JOINT2,0.0,0,-0.217,0,1.0,0.0,1.5708,"
  --conf-file-option "collision_pair: RLEG_JOINT2:LLEG_JOINT2 RLEG_JOINT2:LLEG_JOINT3 RLEG_JOINT2:LLEG_JOINT5 RLEG_JOINT2:RARM_JOINT3 RLEG_JOINT2:RARM_JOINT4 RLEG_JOINT2:RARM_JOINT5 RLEG_JOINT2:RARM_JOINT6 RLEG_JOINT2:LARM_JOINT3 RLEG_JOINT2:LARM_JOINT4 RLEG_JOINT2:LARM_JOINT5 RLEG_JOINT2:LARM_JOINT6 RLEG_JOINT3:LLEG_JOINT2 RLEG_JOINT3:LLEG_JOINT3 RLEG_JOINT3:LLEG_JOINT5 RLEG_JOINT3:RARM_JOINT3 RLEG_JOINT3:RARM_JOINT4 RLEG_JOINT3:RARM_JOINT5 RLEG_JOINT3:RARM_JOINT6 RLEG_JOINT3:LARM_JOINT3 RLEG_JOINT3:LARM_JOINT4 RLEG_JOINT3:LARM_JOINT5 RLEG_JOINT3:LARM_JOINT6 RLEG_JOINT5:LLEG_JOINT2 RLEG_JOINT5:LLEG_JOINT3 RLEG_JOINT5:LLEG_JOINT5 RLEG_JOINT5:RARM_JOINT3 RLEG_JOINT5:RARM_JOINT4 RLEG_JOINT5:RARM_JOINT5 RLEG_JOINT5:RARM_JOINT6 RLEG_JOINT5:LARM_JOINT3 RLEG_JOINT5:LARM_JOINT4 RLEG_JOINT5:LARM_JOINT5 RLEG_JOINT5:LARM_JOINT6 LLEG_JOINT2:RARM_JOINT3 LLEG_JOINT2:RARM_JOINT4 LLEG_JOINT2:RARM_JOINT5 LLEG_JOINT2:RARM_JOINT6 LLEG_JOINT2:LARM_JOINT3 LLEG_JOINT2:LARM_JOINT4 LLEG_JOINT2:LARM_JOINT5 LLEG_JOINT2:LARM_JOINT6 LLEG_JOINT3:RARM_JOINT3 LLEG_JOINT3:RARM_JOINT4 LLEG_JOINT3:RARM_JOINT5 LLEG_JOINT3:RARM_JOINT6 LLEG_JOINT3:LARM_JOINT3 LLEG_JOINT3:LARM_JOINT4 LLEG_JOINT3:LARM_JOINT5 LLEG_JOINT3:LARM_JOINT6 LLEG_JOINT5:RARM_JOINT3 LLEG_JOINT5:RARM_JOINT4 LLEG_JOINT5:RARM_JOINT5 LLEG_JOINT5:RARM_JOINT6 LLEG_JOINT5:LARM_JOINT3 LLEG_JOINT5:LARM_JOINT4 LLEG_JOINT5:LARM_JOINT5 LLEG_JOINT5:LARM_JOINT6 CHEST_JOINT1:RARM_JOINT2 CHEST_JOINT1:RARM_JOINT3 CHEST_JOINT1:RARM_JOINT4 CHEST_JOINT1:RARM_JOINT5 CHEST_JOINT1:RARM_JOINT6 CHEST_JOINT1:LARM_JOINT2 CHEST_JOINT1:LARM_JOINT3 CHEST_JOINT1:LARM_JOINT4 CHEST_JOINT1:LARM_JOINT5 CHEST_JOINT1:LARM_JOINT6 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:RARM_JOINT6 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 HEAD_JOINT1:LARM_JOINT6 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT6 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT2:LARM_JOINT6 RARM_JOINT2:WAIST RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT3:LARM_JOINT6 RARM_JOINT3:WAIST RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT4:LARM_JOINT6 RARM_JOINT4:WAIST RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5 RARM_JOINT5:LARM_JOINT6 RARM_JOINT5:WAIST RARM_JOINT6:LARM_JOINT0 RARM_JOINT6:LARM_JOINT2 RARM_JOINT6:LARM_JOINT3 RARM_JOINT6:LARM_JOINT4 RARM_JOINT6:LARM_JOINT5 RARM_JOINT6:LARM_JOINT6 RARM_JOINT6:WAIST LARM_JOINT2:WAIST LARM_JOINT3:WAIST LARM_JOINT4:WAIST LARM_JOINT5:WAIST LARM_JOINT6:WAIST CHEST_JOINT2:RARM_JOINT3 CHEST_JOINT2:RARM_JOINT4 CHEST_JOINT2:RARM_JOINT5 CHEST_JOINT2:RARM_JOINT6 CHEST_JOINT2:LARM_JOINT3 CHEST_JOINT2:LARM_JOINT4 CHEST_JOINT2:LARM_JOINT5 CHEST_JOINT2:LARM_JOINT6 CHEST_JOINT2:LARM_JOINT2 CHEST_JOINT2:RARM_JOINT2"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
  --simulation-joint-properties-option "RARM_JOINT1.angle,-0.191986,LARM_JOINT1.angle,0.191986"
)
gen_minmax_table_for_closed_robots(JAXON_BLUE JAXON_BLUE JAXON_BLUE)

# CHIDORI
compile_rbrain_model_for_closed_robots(CHIDORI CHIDORI CHIDORI
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  ## do not use CHIDORI.conf for real robot
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/CHIDORI.PDgains_sim.dat"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.1065,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.1065,0.0,0.0,0.0,0.0,"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --conf-file-option "collision_loop: 20"
  --conf-file-option "collision_pair: WAIST:LLEG_JOINT2 WAIST:RLEG_JOINT2 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5"
  --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)"
  --conf-file-option "seq_optional_data_dim: 4"
  )

# TQLEG0
compile_rbrain_model_for_closed_robots(TQLEG0 TQLEG0 TQLEG0
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "abc_leg_offset: 0.0, 0.1, 0.0"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.0,0.0,-0.110,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.0,0.0,-0.110,0.0,0.0,0.0,0.0,"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  --conf-file-option "collision_loop: 20"
  --conf-file-option "collision_pair: LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5"
  --conf-file-option "# SequencePlayer optional data (contactStates x 2 + controlSwingTime x 2 (2 is rfsensor, lfsensor)"
  --conf-file-option "seq_optional_data_dim: 4"
  )

# A0B
compile_rbrain_model_for_closed_robots(A0B A0B A0B
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "end_effectors: rarm,RARM_JOINT5,WAIST,0.25375,0.0,0.0,0.0,0.0,0.0,0.0,"
  --conf-dt-option "0.002"
  --simulation-timestep-option "0.002"
  )

if(EXISTS ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl)
  compile_openhrp_model(
    ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl
    TESTMDOFARM)
endif()

# TABLIS
compile_rbrain_model_for_closed_robots(TABLIS TABLIS TABLIS
  --conf-dt-option "0.001"
  --simulation-timestep-option "0.001"
  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
  --conf-file-option "pdgains_sim_file_name: ${PROJECT_SOURCE_DIR}/models/TABLIS.PDgains_sim.dat"
  --conf-file-option "abc_leg_offset: 0.0, 0.16, 0.0"
  --conf-file-option "end_effectors: rleg,RLEG_JOINT5,WAIST,0.12,0.0,-0.1062,0.0,0.0,0.0,0.0, lleg,LLEG_JOINT5,WAIST,0.12,0.0,-0.1062,0.0,0.0,0.0,0.0, rarm,RARM_JOINT6,WAIST,0.113,0.0,-0.08,0.0,1.0,0.0,1.5708, larm,LARM_JOINT6,WAIST,0.113,0.0,-0.08,0.0,1.0,0.0,1.5708, relbow,RARM_JOINT7,WAIST,0.0,0.0,0.25,0.0,1.0,0.0,1.5708, lelbow,LARM_JOINT7,WAIST,0.0,0.0,0.25,0.0,1.0,0.0,1.5708,"
  --conf-file-option "collision_pair: WAIST:LLEG_JOINT2 WAIST:RLEG_JOINT2 LLEG_JOINT2:LLEG_JOINT5 LLEG_JOINT2:RLEG_JOINT2 LLEG_JOINT2:RLEG_JOINT3 LLEG_JOINT2:RLEG_JOINT5 LLEG_JOINT3:RLEG_JOINT2 LLEG_JOINT3:RLEG_JOINT3 LLEG_JOINT3:RLEG_JOINT5 LLEG_JOINT5:RLEG_JOINT2 LLEG_JOINT5:RLEG_JOINT3 LLEG_JOINT5:RLEG_JOINT5 RLEG_JOINT2:RLEG_JOINT5"
  --conf-file-option "collision_model: convex hull"
  --conf-file-option "# SequencePlayer optional data (contactStates x 4 + controlSwingTime x 4 (4 is rfsensor, lfsensor, rhsensor, lhsensor)"
  --conf-file-option "seq_optional_data_dim: 8"
)
#gen_minmax_table_for_closed_robots(TABLIS TABLIS TABLIS)

################################
## Generate default simulation launch files and euslisp interface files
################################
macro (generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots ROBOT_DIR ROBOT_NAME)
  set(_arg_list ${ARGV})
  # remove arguments of this macro
  list(REMOVE_AT _arg_list 0 1)
  if(EXISTS ${hrp2_models_MODEL_DIR}/${ROBOT_DIR}/${ROBOT_NAME}main.wrl)
    generate_default_launch_eusinterface_files("$(find hrp2_models)/${ROBOT_DIR}/${ROBOT_NAME}main.wrl" hrpsys_ros_bridge_tutorials ${ROBOT_NAME} ${_arg_list})
  endif()
endmacro ()
macro (generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots ROBOT_DIR ROBOT_NAME)
  set(_arg_list ${ARGV})
  # remove arguments of this macro
  list(REMOVE_AT _arg_list 0 1)
  if(EXISTS ${jsk_models_MODEL_DIR}/${ROBOT_DIR}/${ROBOT_NAME}main.wrl)
    generate_default_launch_eusinterface_files("$(find jsk_models)/${ROBOT_DIR}/${ROBOT_NAME}main.wrl" hrpsys_ros_bridge_tutorials ${ROBOT_NAME} ${_arg_list})
  endif()
endmacro ()

generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK_for_OpenHRP3 HRP2JSK "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNT_for_OpenHRP3 HRP2JSKNT "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSKNTS_for_OpenHRP3 HRP2JSKNTS "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2W_for_OpenHRP3 HRP2W "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2G_for_OpenHRP3 HRP2G "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP4R HRP4R "--use-unstable-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(STARO STARO "--use-unstable-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(JAXON JAXON "--use-robot-hrpsys-config")
if("$ENV{TRAVIS_JOB_ID}" STREQUAL "")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(JAXON_RED JAXON_RED "--use-robot-hrpsys-config")
endif()
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(JAXON_BLUE JAXON_BLUE "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(URATALEG URATALEG "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(YSTLEG YSTLEG "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(CHIDORI CHIDORI "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TQLEG0 TQLEG0 "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(A0B A0B "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TABLIS TABLIS "--use-robot-hrpsys-config")
generate_default_launch_eusinterface_files(
  "$(find hrpsys_ros_bridge_tutorials)/models/TESTMDOFARM.wrl"
  hrpsys_ros_bridge_tutorials
  TESTMDOFARM
  "--no-euslisp")
generate_default_launch_eusinterface_files("$(find openhrp3)/share/OpenHRP-3.1/sample/model/sample1.wrl" hrpsys_ros_bridge_tutorials SampleRobot "--use-robot-hrpsys-config")

################################
## Generate and modify .urdf files
##   Org file : ROBOT.urdf, ROBOT.urdf.xacro
##   generate_hand_attached_xx_model : Generate ROBOT_body.urdf from ROBOT.urdf, ROBOT.urdf.xacro
##   run_xacro_for_hand_hrp2_model : Generate ROBOT_WH.urdf from ROBOT_body.urdf
##   attach_sensor_and_endeffector_to_hrp2jsk_urdf : Generate ROBOT_WH_SESNROS.urdf from ROBOT_WR.urdf
################################
# find xacro
find_package(xacro)
if(EXISTS ${xacro_SOURCE_PREFIX}/xacro.py)
  set(xacro_exe ${xacro_SOURCE_PREFIX}/xacro.py)
else()
  set(xacro_exe ${xacro_PREFIX}/share/xacro/xacro.py)
endif()

find_package(euscollada)
if (euscollada_SOURCE_PREFIX)
  set(euscollada_PACKAGE_PATH ${euscollada_SOURCE_PREFIX})
else ()
  set(euscollada_PACKAGE_PATH ${euscollada_PREFIX}/share/euscollada)
endif()

macro (generate_hand_attached_hrp2_model _robot_name)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_urdf_file "${_model_dir}/${_robot_name}.urdf")
  set(_out_urdf_file "${_model_dir}/${_robot_name}_body.urdf")
  set(_script_file "${euscollada_PACKAGE_PATH}/scripts/remove_sensor_from_urdf.py")
  string(TOLOWER ${_robot_name} _srobot_name)
  message("generate hand_attached_hrp2_model for ${_robot_name}")
  add_custom_command(OUTPUT ${_out_urdf_file}
      COMMAND ${_script_file}
      LARM_LINK6 RARM_LINK6 ${_in_urdf_file} ${_out_urdf_file}
      DEPENDS ${_in_urdf_file} ${_script_file} ${_srobot_name}_${PROJECT_NAME}_compile_urdf)
endmacro()

macro (run_xacro_for_hand_hrp2_model _robot_name)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_xacro_file "${_model_dir}/${_robot_name}.urdf.xacro")
  set(_in_body_urdf "${_model_dir}/${_robot_name}_body.urdf")
  set(_in_hand_l_urdf "${_model_dir}/HRP3HAND_L.urdf")
  set(_in_hand_r_urdf "${_model_dir}/HRP3HAND_R.urdf")
  set(_out_urdf_file "${_model_dir}/${_robot_name}_WH.urdf")
  add_custom_command(OUTPUT ${_out_urdf_file}
      COMMAND ${xacro_exe} ${_in_xacro_file} -o ${_out_urdf_file}
      DEPENDS ${_in_xacro_file} ${_in_body_urdf}
      ${_in_hand_l_urdf} ${_in_hand_r_urdf})
endmacro()

macro (attach_sensor_and_endeffector_to_hrp2jsk_urdf
    _robot_name _urdf_file _out_file _yaml_file)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_urdf_file "${_model_dir}/${_urdf_file}")
  set(_in_yaml_file "${_model_dir}/${_yaml_file}")
  set(_out_urdf_file "${_model_dir}/${_out_file}")
  set(_script_file ${euscollada_PACKAGE_PATH}/scripts/add_sensor_to_collada.py)
  add_custom_command(OUTPUT ${_out_urdf_file}
    COMMAND ${_script_file}
    ${_in_urdf_file} -O ${_out_urdf_file} -C ${_in_yaml_file}
    DEPENDS ${_in_urdf_file} ${_in_yaml_file} ${_script_file})
  list(APPEND compile_urdf_robots ${_out_urdf_file})
endmacro()

# for HRP2JSK + multisense
pkg_check_modules(multisense_description multisense_description QUIET)
if(${hrp2_models_FOUND} AND multisense_description_FOUND)
  # generate HRP2JSK_WH.urdf from HRP2JSK.urdf.xacro
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  add_custom_command(OUTPUT ${_model_dir}/HRP2JSK_WH.urdf
    COMMAND ${xacro_exe} ${_model_dir}/HRP2JSK.urdf.xacro -o ${_model_dir}/HRP2JSK_WH.urdf
    DEPENDS ${_model_dir}/HRP2JSK.urdf.xacro ${_model_dir}/HRP2JSK.urdf)
  # generate HRP2JSK_WH_SENSORS.urdf from HRP2JSK_WH.urdf and yaml
  attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSK HRP2JSK_WH.urdf
    HRP2JSK_WH_SENSORS.urdf hrp2jsk.yaml)
endif(${hrp2_models_FOUND} AND multisense_description_FOUND)

# for HRP2W + multisense
pkg_check_modules(multisense_description multisense_description QUIET)
if(${hrp2_models_FOUND} AND multisense_description_FOUND)
  # generate HRP2W_WH.urdf from HRP2W.urdf.xacro
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  add_custom_command(OUTPUT ${_model_dir}/HRP2W_WH.urdf
    COMMAND ${xacro_exe} ${_model_dir}/HRP2W.urdf.xacro -o ${_model_dir}/HRP2W_WH.urdf
    DEPENDS ${_model_dir}/HRP2W.urdf.xacro ${_model_dir}/HRP2W.urdf)
  # generate HRP2W_WH_SENSORS.urdf from HRP2W_WH.urdf and yaml
  attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2W HRP2W_WH.urdf
    HRP2W_WH_SENSORS.urdf hrp2w.yaml)
endif(${hrp2_models_FOUND} AND multisense_description_FOUND)

# for HRP2G + multisense
pkg_check_modules(multisense_description multisense_description QUIET)
if(${hrp2_models_FOUND} AND multisense_description_FOUND)
  # generate HRP2G_WH.urdf from HRP2G.urdf.xacro
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  add_custom_command(OUTPUT ${_model_dir}/HRP2G_WH.urdf
    COMMAND ${xacro_exe} ${_model_dir}/HRP2G.urdf.xacro -o ${_model_dir}/HRP2G_WH.urdf
    DEPENDS ${_model_dir}/HRP2G.urdf.xacro ${_model_dir}/HRP2G.urdf)
  # generate HRP2G_WH_SENSORS.urdf from HRP2G_WH.urdf and yaml
  attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2G HRP2G_WH.urdf
    HRP2G_WH_SENSORS.urdf hrp2g.yaml)
endif(${hrp2_models_FOUND} AND multisense_description_FOUND)

# for HRP2JSKNT, HRP2JSKNTS + multisense
if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl)
  generate_hand_attached_hrp2_model(HRP2JSKNT)
  generate_hand_attached_hrp2_model(HRP2JSKNTS)
  run_xacro_for_hand_hrp2_model(HRP2JSKNT)
  pkg_check_modules(multisense_description multisense_description QUIET)
  if(multisense_description_FOUND)
    run_xacro_for_hand_hrp2_model(HRP2JSKNTS)
    attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNTS HRP2JSKNTS_WH.urdf
      HRP2JSKNTS_WH_SENSORS.urdf hrp2jsknts.yaml)
    attach_sensor_and_endeffector_to_hrp2jsk_urdf(HRP2JSKNT HRP2JSKNT_WH.urdf
      HRP2JSKNT_WH_SENSORS.urdf hrp2jsknt.yaml)
  endif(multisense_description_FOUND)
endif()

# for HIRONXJSK (adding sensor, and fixing some points for gazebo)
if(${hrp2_models_FOUND})
  attach_sensor_and_endeffector_to_hrp2jsk_urdf(HIRONXJSK HIRONXJSK.urdf
    HIRONXJSK_SENSORS.urdf hironxjsk.yaml)
endif(${hrp2_models_FOUND})

macro (generate_staro_hand_model _robot_name _dir_name)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_org_model_dir "${jsk_models_MODEL_DIR}/${_dir_name}/")
  set(_in_urdf_file "${_model_dir}/${_robot_name}.urdf")
  set(_out_mesh_dir "${_model_dir}/${_robot_name}_meshes/")
  set(_l_mesh "l_hand_attached_link.dae")
  set(_r_mesh "r_hand_attached_link.dae")
  # set(_h_mesh "HEAD_LINK1_without_camera.dae")
  add_custom_command(OUTPUT ${_out_mesh_dir}/${_l_mesh}
    COMMAND cp ${_org_model_dir}/${_l_mesh} ${_out_mesh_dir}/${_l_mesh}
    DEPENDS ${_in_urdf_file})
  add_custom_command(OUTPUT ${_out_mesh_dir}/${_r_mesh}
    COMMAND cp ${_org_model_dir}/${_r_mesh} ${_out_mesh_dir}/${_r_mesh}
    DEPENDS ${_in_urdf_file})
  #add_custom_command(OUTPUT ${_out_mesh_dir}/${_h_mesh}
  #  COMMAND cp ${_org_model_dir}/${_h_mesh} ${_out_mesh_dir}/${_h_mesh}
  #  DEPENDS ${_in_urdf_file})
  #add_custom_target(${_robot_name}_head_generate DEPENDS ${_out_mesh_dir}/${_h_mesh})
  list(APPEND compile_urdf_robots ${_out_mesh_dir}/${_l_mesh})
  list(APPEND compile_urdf_robots ${_out_mesh_dir}/${_r_mesh})
  #list(APPEND compile_${_robot_name}_urdf_robots ${_robot_name}_head_generate)
endmacro()

macro (generate_hand_attached_staro_model _robot_name)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_urdf_file "${_model_dir}/${_robot_name}.urdf")
  set(_out_urdf_file "${_model_dir}/${_robot_name}_body.urdf")
  string(TOLOWER ${_robot_name} _srobot_name)
  set(_script_file "${PROJECT_SOURCE_DIR}/models/gen_hand_attached_staro_model.sh")
  message("generate hand_attached_staro_model for ${_robot_name}")
  add_custom_command(OUTPUT ${_out_urdf_file}
      COMMAND ${_script_file} ${_robot_name} ${_in_urdf_file} ${euscollada_PACKAGE_PATH}
      DEPENDS ${_in_urdf_file} ${_srobot_name}_${PROJECT_NAME}_compile_urdf)
endmacro()

macro (run_xacro_for_hand_staro_model _robot_name)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_xacro_file "${_model_dir}/${_robot_name}.urdf.xacro")
  set(_in_body_urdf "${_model_dir}/${_robot_name}_body.urdf")
  set(_out_urdf_file "${_model_dir}/${_robot_name}_WH.urdf")
  add_custom_command(OUTPUT ${_out_urdf_file}
      COMMAND ${xacro_exe} ${_in_xacro_file} -o ${_out_urdf_file}
      DEPENDS ${_in_xacro_file} ${_in_body_urdf})
endmacro()

macro (attach_sensor_and_endeffector_to_staro_urdf
    _robot_name _urdf_file _out_file _yaml_file)
  set(_model_dir "${PROJECT_SOURCE_DIR}/models/")
  set(_in_urdf_file "${_model_dir}${_urdf_file}")
  set(_in_yaml_file "${_model_dir}${_yaml_file}")
  set(_out_urdf_file "${_model_dir}${_out_file}")
  set(_script_file "${_model_dir}gen_sensor_attached_staro_model.sh")
  add_custom_command(OUTPUT ${_out_urdf_file}
    COMMAND ${_script_file} ${_robot_name} ${euscollada_PACKAGE_PATH} ${_in_urdf_file} ${_out_urdf_file} ${_in_yaml_file}
    DEPENDS ${_in_urdf_file} ${_in_yaml_file} ${_script_file})
  list(APPEND compile_urdf_robots ${_out_urdf_file})
endmacro()

if(EXISTS ${jsk_models_MODEL_DIR}/STARO/l_hand_attached_link.dae)
  find_package(PkgConfig)
  pkg_check_modules(multisense_description multisense_description QUIET)
  find_package(robotiq_hand_description QUIET)
  if(multisense_description_FOUND AND robotiq_hand_description_FOUND)
    generate_staro_hand_model(STARO STARO)
    generate_hand_attached_staro_model(STARO)
    run_xacro_for_hand_staro_model(STARO)
    attach_sensor_and_endeffector_to_staro_urdf(STARO STARO_WH.urdf STARO_WH_SENSORS.urdf staro.yaml)
  endif()
endif()

if(EXISTS ${jsk_models_MODEL_DIR}/JAXON/l_hand_attached_link.dae)
  find_package(PkgConfig)
  find_package(multisense_description QUIET)
  if(multisense_description_FOUND)
    find_package(jaxon_description)
    if(NOT jaxon_description_SOURCE_DIR AND jaxon_description_SOURCE_PREFIX)
      set(jaxon_description_SOURCE_DIR ${jaxon_description_SOURCE_PREFIX})
    endif(NOT jaxon_description_SOURCE_DIR AND jaxon_description_SOURCE_PREFIX)
    find_package(jaxon_description)
    if(EXISTS ${jaxon_description_SOURCE_DIR})
      generate_staro_hand_model(JAXON JAXON)
      generate_hand_attached_staro_model(JAXON)
      run_xacro_for_hand_staro_model(JAXON)
      attach_sensor_and_endeffector_to_staro_urdf(JAXON JAXON_WH.urdf JAXON_WH_SENSORS.urdf jaxon.yaml)
    endif()
  endif()
endif()

if("$ENV{TRAVIS_JOB_ID}" STREQUAL "")
if(EXISTS ${jsk_models_MODEL_DIR}/JAXON_RED/l_hand_attached_link.dae)
  find_package(PkgConfig)
  find_package(multisense_description QUIET)
  if(multisense_description_FOUND)
    find_package(jaxon_description)
    if(NOT jaxon_description_SOURCE_DIR AND jaxon_description_SOURCE_PREFIX)
      set(jaxon_description_SOURCE_DIR ${jaxon_description_SOURCE_PREFIX})
    endif(NOT jaxon_description_SOURCE_DIR AND jaxon_description_SOURCE_PREFIX)
    find_package(jaxon_description)
    if(EXISTS ${jaxon_description_SOURCE_DIR})
      generate_staro_hand_model(JAXON_RED JAXON)
      generate_hand_attached_staro_model(JAXON_RED)
      run_xacro_for_hand_staro_model(JAXON_RED)
      attach_sensor_and_endeffector_to_staro_urdf(JAXON_RED JAXON_RED_WH.urdf JAXON_RED_WH_SENSORS.urdf jaxon_red.yaml)
    endif()
  endif()
endif()
endif()

if(${jsk_models_FOUND})
  attach_sensor_and_endeffector_to_staro_urdf(JAXON_BLUE JAXON_BLUE.urdf JAXON_BLUE_SENSORS.urdf jaxon_blue.yaml)
  attach_sensor_and_endeffector_to_staro_urdf(CHIDORI CHIDORI.urdf CHIDORI_SENSORS.urdf chidori.yaml)
  attach_sensor_and_endeffector_to_staro_urdf(TABLIS TABLIS.urdf TABLIS_SENSORS.urdf tablis.yaml)
endif()

# for SampleRobot
attach_sensor_and_endeffector_to_hrp2jsk_urdf(SampleRobot SampleRobot.urdf
  SampleRobot_WH_SENSORS.urdf samplerobot.yaml)

# Define dependency for urdf conversion to avoid parallel cmake problem (https://github.com/start-jsk/rtmros_tutorials/issues/373)
if (DEFINED compile_urdf_robots)
  add_custom_target(all_urdf_model_generate ALL DEPENDS ${compile_urdf_robots})
endif()

install(DIRECTORY euslisp launch scripts models test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
install(CODE
  "execute_process(COMMAND echo \"fix \$ENV{DESTDIR}/${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/model/* ${CATKIN_DEVEL_PREFIX} -> ${CMAKE_INSTALL_PREFIX}\")
   file(GLOB _conf_files \"\$ENV{DISTDIR}/${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/model/*/*.conf\")
   foreach(_conf_file \${_conf_files})
     execute_process(COMMAND sed -i s@${CATKIN_DEVEL_PREFIX}@${CMAKE_INSTALL_PREFIX}@g \${_conf_file})
     execute_process(COMMAND sed -i s@${hrpsys_ros_bridge_tutorials_SOURCE_DIR}@${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}@g \${_conf_file})
  endforeach()
")

add_rostest(test/test_hrpsys_pa10.launch)
add_rostest(test/test_hrpsys_samplerobot.launch)
#add_rostest(test/samplerobot_hrpsys-ros-bridge_test.launch)
if(${hrp2_models_FOUND})
  add_rostest(test/test_robot_model.launch)
endif()