From 01c62a7822b17b451a2684390753ed7992212567 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 26 May 2018 18:43:02 +0900 Subject: [PATCH 1/4] Auto-generate Euslisp interface of HIRONX --- hrpsys_ros_bridge_tutorials/CMakeLists.txt | 6 +++ .../models/hironx.yaml | 46 +++++++++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 hrpsys_ros_bridge_tutorials/models/hironx.yaml 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/models/hironx.yaml b/hrpsys_ros_bridge_tutorials/models/hironx.yaml new file mode 100644 index 00000000..130fce33 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/models/hironx.yaml @@ -0,0 +1,46 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - RARM_JOINT0 : rarm-shoulder-y + - RARM_JOINT1 : rarm-shoulder-p + - RARM_JOINT2 : rarm-elbow-p + - RARM_JOINT3 : rarm-wrist-y + - RARM_JOINT4 : rarm-wrist-p + - RARM_JOINT5 : rarm-wrist-r +larm: + - LARM_JOINT0 : larm-shoulder-y + - LARM_JOINT1 : larm-shoulder-p + - LARM_JOINT2 : larm-elbow-p + - LARM_JOINT3 : larm-wrist-y + - LARM_JOINT4 : larm-wrist-p + - LARM_JOINT5 : larm-wrist-r +torso: + - CHEST_JOINT0 : torso-waist-y +head: + - HEAD_JOINT0 : head-neck-y + - HEAD_JOINT1 : head-neck-p + +## +## end-coords +## +larm-end-coords: + translate : [-0.05, 0, 0] + rotate : [0, 1, 0, 90] +rarm-end-coords: + translate : [-0.05, 0, 0] + rotate : [0, 1, 0, 90] +head-end-coords: + translate : [0.10, 0, 0.10] + rotate : [0, 1, 0, 90] + +## +## reset-pose +## +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] + + + From c4f909ba0ea8d496aa7d697c8c8ec90b7555e7a8 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 28 May 2018 19:41:45 +0900 Subject: [PATCH 2/4] Remove unused lisp files of hironx --- .../euslisp/kawada-hironx-example.l | 17 ------- .../euslisp/kawada-hironx-interface.l | 15 ------ .../models/kawada-hironx.yaml | 46 ------------------- 3 files changed, 78 deletions(-) delete mode 100644 hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-example.l delete mode 100644 hrpsys_ros_bridge_tutorials/euslisp/kawada-hironx-interface.l delete mode 100644 hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml 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/models/kawada-hironx.yaml b/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml deleted file mode 100644 index 130fce33..00000000 --- a/hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml +++ /dev/null @@ -1,46 +0,0 @@ -## -## - collada_joint_name : euslisp_joint_name (start with :) -## - -rarm: - - RARM_JOINT0 : rarm-shoulder-y - - RARM_JOINT1 : rarm-shoulder-p - - RARM_JOINT2 : rarm-elbow-p - - RARM_JOINT3 : rarm-wrist-y - - RARM_JOINT4 : rarm-wrist-p - - RARM_JOINT5 : rarm-wrist-r -larm: - - LARM_JOINT0 : larm-shoulder-y - - LARM_JOINT1 : larm-shoulder-p - - LARM_JOINT2 : larm-elbow-p - - LARM_JOINT3 : larm-wrist-y - - LARM_JOINT4 : larm-wrist-p - - LARM_JOINT5 : larm-wrist-r -torso: - - CHEST_JOINT0 : torso-waist-y -head: - - HEAD_JOINT0 : head-neck-y - - HEAD_JOINT1 : head-neck-p - -## -## end-coords -## -larm-end-coords: - translate : [-0.05, 0, 0] - rotate : [0, 1, 0, 90] -rarm-end-coords: - translate : [-0.05, 0, 0] - rotate : [0, 1, 0, 90] -head-end-coords: - translate : [0.10, 0, 0.10] - rotate : [0, 1, 0, 90] - -## -## reset-pose -## -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] - - - From 60a226a5adb044b2dab5abdbf6ac6392106a2ff2 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 28 May 2018 19:53:45 +0900 Subject: [PATCH 3/4] Remove hironx launches which are auto-generated --- .../launch/hironx.launch | 44 ------------------ .../launch/hironx_nosim.launch | 45 ------------------- .../launch/hironx_ros_bridge.launch | 17 ------- .../launch/hironx_startup.launch | 20 --------- 4 files changed, 126 deletions(-) delete mode 100644 hrpsys_ros_bridge_tutorials/launch/hironx.launch delete mode 100644 hrpsys_ros_bridge_tutorials/launch/hironx_nosim.launch delete mode 100644 hrpsys_ros_bridge_tutorials/launch/hironx_ros_bridge.launch delete mode 100644 hrpsys_ros_bridge_tutorials/launch/hironx_startup.launch 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 @@ - - - - - - - - - - - - - - - - - - - - From d992a5aeac4078a085832707bd4a2cb7f1d96f79 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 25 Jun 2018 13:19:56 +0900 Subject: [PATCH 4/4] Add _InitialPose in py API as reset-manip-pose --- hrpsys_ros_bridge_tutorials/models/hironx.yaml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/models/hironx.yaml b/hrpsys_ros_bridge_tutorials/models/hironx.yaml index 130fce33..e0e6d122 100644 --- a/hrpsys_ros_bridge_tutorials/models/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]