diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 5e2b7745..c1b61b8c 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -277,6 +277,11 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl) HRP3HAND_R) endif() +# HIRONX +compile_openhrp_model_for_closed_robots(HIRONX HIRONX HIRONX + --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," + ) + # URATALEG compile_rbrain_model_for_closed_robots(URATALEG URATALEG URATALEG --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" @@ -459,6 +464,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_openhrp_robots(HRP2JSK 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_openhrp_robots(HIRONX HIRONX "--use-robot-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 "") diff --git a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l b/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l deleted file mode 100644 index 66c7a1be..00000000 --- a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l +++ /dev/null @@ -1,17 +0,0 @@ -(ros::roseus "kawadahironx_pickbox") -(load "package://hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l") - -(kawada-hironx-init) -(dotimes (i 10) - (send *hiro* :reset-pose) - ;;(objects (list *hiro*)) - (send *ri* :angle-vector (send *hiro* :angle-vector) 3000) - (send *ri* :wait-interpolation) - (send *hiro* :rarm :shoulder-p :joint-angle -40) - (send *hiro* :rarm :elbow-p :joint-angle -130) - (send *hiro* :head :neck-y :joint-angle -20) - ;;(objects (list *hiro*)) - (send *ri* :angle-vector (send *hiro* :angle-vector) 3000) - (send *ri* :wait-interpolation) - ) -(warning-message 2 ";; type (exit) to finish program~%") diff --git a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l deleted file mode 100644 index 2467be0e..00000000 --- a/hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l +++ /dev/null @@ -1,15 +0,0 @@ -(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") -(require :kawadahironx "package://hrpsys_ros_bridge_tutorials/models/kawada-hironx.l") - -(defclass kawadahironx-interface - :super rtm-ros-robot-interface - :slots ()) -(defmethod kawadahironx-interface - (:init (&rest args) - (send-super :init :robot hironx-robot))) - -(defun kawada-hironx-init (&rest args) - (if (not (boundp '*ri*)) - (setq *ri* (instance* kawadahironx-interface :init args))) - (if (not (boundp '*hiro*)) - (setq *hiro* (instance hironx-robot :init)))) diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx.launch b/hrpsys_ros_bridge_tutorials/launch/hironx.launch deleted file mode 100644 index 825acad2..00000000 --- a/hrpsys_ros_bridge_tutorials/launch/hironx.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch deleted file mode 100644 index 15367dea..00000000 --- a/hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch deleted file mode 100644 index 5a50982b..00000000 --- a/hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - diff --git a/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch b/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch deleted file mode 100644 index 1c63f32e..00000000 --- a/hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml b/hrpsys_ros_bridge_tutorials/models/hironx.yaml similarity index 84% rename from hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml rename to hrpsys_ros_bridge_tutorials/models/hironx.yaml index 130fce33..e0e6d122 100644 --- a/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml +++ b/hrpsys_ros_bridge_tutorials/models/hironx.yaml @@ -41,6 +41,5 @@ head-end-coords: angle-vector: reset-pose : [0.0, -40.0, -90.0, 0.0, 0.0, 0.0, 0.0, -40.0, -90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30.0] off-pose : [0.0,-140.0,-158.0, 0.0, 0.0, 0.0, 0.0,-140.0,-158.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - - + ## _InitialPose in hironx_client.py in hironx_ros_bridge (used in goInitial()) + reset-manip-pose : [-0.6, 0.0, -100.0, 15.2, 9.4, 3.2, 0.6, 0.0, -100.0, -15.2, 9.4, -3.2, 0.0, 0.0, 0.0]