From 01c62a7822b17b451a2684390753ed7992212567 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Sat, 26 May 2018 18:43:02 +0900 Subject: [PATCH 1/9] 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/9] 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/9] 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/9] 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] From 8127a146520000dcebae92c4c8b4b85746dee6ab Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 21 Jun 2018 19:27:34 +0900 Subject: [PATCH 5/9] Add hironx-interface supporting impedance --- .../euslisp/hironx-interface.l | 103 ++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l new file mode 100644 index 00000000..c37aeb37 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l @@ -0,0 +1,103 @@ +(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") +(require :hironx "package://hrpsys_ros_bridge_tutorials/models/hironx.l") +(when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hironx-utils.l")) + (require :hironx-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hironx-utils.l")) +(if (ros::resolve-ros-path "package://hironx_ros_bridge") + (ros::load-ros-manifest "hironx_ros_bridge")) + +(defclass hironx-interface + :super rtm-ros-robot-interface + :slots ()) + +;; Initialize +(defmethod hironx-interface + (:init (&rest args) + (send-super* :init :robot hironx-robot args)) + (:define-all-ROSBridge-srv-methods + (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge")) + ;; First, define ROSBridge method for old impedance controller + (if (ros::resolve-ros-path "package://hironx_ros_bridge") + (send-super :define-all-ROSBridge-srv-methods :ros-pkg-name "hironx_ros_bridge")) + ;; Second, define ROSBridge method based on hrpsys_ros_bridge + ;; Method created already is not overwritten, so we can keep using old impedance controller + ;; See :get-ROSBridge-method-def-macro + (send-super :define-all-ROSBridge-srv-methods))) + +;; ImpedanceControllerService +;; Based on hironx_client.py in hironx_ros_bridge and rtm-ros-robot-interface.l. +;; Methods executable with old impedance controller. +;; The reason why I don't use def-set-get-param-method is that +;; OpenHRP_ImpedanceControllerService_setImpedanceControllerParam.srv has element "name" inside i_param, +;; while OpenHRP_ImpedanceControllerService_getImpedanceControllerParam.srv has that element directly. +;; Set optional-args as (list :name 'name) -> multiple declaration of variable "name" in set-param-method. +;; Set optional-args as nil -> pass nothing in service call of get-param-method +(defmethod hironx-interface + (:get-impedance-controller-param (limb) + (let (sensor-name) + (cond ((eq limb :rarm) + (setq sensor-name "rhsensor")) + ((eq limb :larm) + (setq sensor-name "lhsensor")) + ((eq limb :arms) + (return-from :get-impedance-controller-param + (mapcar #'(lambda (l) (send self :get-impedance-controller-param l)) + '(:rarm :larm)))) + (t (error ";; No such limb: ~A~%." limb))) + (send (send self :impedancecontrollerservice_getimpedancecontrollerparam :name sensor-name) + :i_param))) + (:start-impedance + (limb &rest args &key (m-p 100) (d-p 100) (k-p 100) (m-r 100) (d-r 2000) (k-r 2000) + (ref-force #f(0 0 0)) (force-gain #f(1 1 1)) (ref-moment #f(0 0 0)) + (moment-gain #f(0 0 0)) (sr-gain 1) (avoid-gain 0) (reference-gain 0) + (manipulability-limit 0.1)) + "Start impedance controller mode. + limb should be limb symbol name such as :rarm, :larm, or :arms." + (let (sensor-name target-name) + (cond ((eq limb :rarm) + (setq sensor-name "rhsensor" target-name "RARM_JOINT5")) + ((eq limb :larm) + (setq sensor-name "lhsensor" target-name "LARM_JOINT5")) + ((eq limb :arms) + (return-from :start-impedance + (mapcar #'(lambda (l) (send* self :start-impedance l args)) + '(:rarm :larm)))) + (t (error ";; No such limb: ~A~%." limb))) + (send self :impedancecontrollerservice_setimpedancecontrollerparam :i_param + (instance hironx_ros_bridge::OpenHRP_ImpedanceControllerService_impedanceParam :init + :name sensor-name :base_name "CHEST_JOINT0" :target_name target-name + :m_p m-p :d_p d-p :k_p k-p :m_r m-r :d_r d-r :k_r k-r :ref_force ref-force + :force_gain force-gain :ref_moment ref-moment :moment_gain moment-gain + :sr_gain sr-gain :avoid_gain avoid-gain :reference_gain reference-gain + :manipulability_limit manipulability-limit)))) + (:stop-impedance (limb) + "Stop impedance controller mode. + limb should be limb symbol name such as :rarm, :larm, or :arms." + (let (sensor-name) + (cond ((eq limb :rarm) + (setq sensor-name "rhsensor")) + ((eq limb :larm) + (setq sensor-name "lhsensor")) + ((eq limb :arms) + (return-from :stop-impedance + (mapcar #'(lambda (l) (send self :stop-impedance l)) + '(:rarm :larm)))) + (t (error ";; No such limb: ~A~%." limb))) + (send self :impedancecontrollerservice_deleteimpedancecontrollerandwait :name sensor-name))) + (:stop-impedance-no-wait (limb) + (let (sensor-name) + (cond ((eq limb :rarm) + (setq sensor-name "rhsensor")) + ((eq limb :larm) + (setq sensor-name "lhsensor")) + ((eq limb :arms) + (return-from :stop-impedance-no-wait + (mapcar #'(lambda (l) (send self :stop-impedance-no-wait l)) + '(:rarm :larm)))) + (t (error ";; No such limb: ~A~%." limb))) + (send self :impedancecontrollerservice_deleteimpedancecontroller :name sensor-name)))) + +(defun hironx-init (&rest args) + (if (not (boundp '*ri*)) + (setq *ri* (instance* hironx-interface :init args))) + (if (not (boundp '*hironx*)) + (setq *hironx* (instance hironx-robot :init)))) From d27647bdda8817ec70965fee4c3b6080877309e3 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 22 Jun 2018 00:04:35 +0900 Subject: [PATCH 6/9] Disable impedance methods unexecutable with hironx --- .../euslisp/hironx-interface.l | 48 ++++++++++++------- 1 file changed, 32 insertions(+), 16 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l index c37aeb37..d48c1f7f 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l @@ -25,26 +25,17 @@ ;; ImpedanceControllerService ;; Based on hironx_client.py in hironx_ros_bridge and rtm-ros-robot-interface.l. -;; Methods executable with old impedance controller. +;; Enable methods executable with old impedance controller and disable others. ;; The reason why I don't use def-set-get-param-method is that ;; OpenHRP_ImpedanceControllerService_setImpedanceControllerParam.srv has element "name" inside i_param, ;; while OpenHRP_ImpedanceControllerService_getImpedanceControllerParam.srv has that element directly. ;; Set optional-args as (list :name 'name) -> multiple declaration of variable "name" in set-param-method. -;; Set optional-args as nil -> pass nothing in service call of get-param-method +;; Set optional-args as nil -> pass nothing in service call of get-param-method. (defmethod hironx-interface - (:get-impedance-controller-param (limb) - (let (sensor-name) - (cond ((eq limb :rarm) - (setq sensor-name "rhsensor")) - ((eq limb :larm) - (setq sensor-name "lhsensor")) - ((eq limb :arms) - (return-from :get-impedance-controller-param - (mapcar #'(lambda (l) (send self :get-impedance-controller-param l)) - '(:rarm :larm)))) - (t (error ";; No such limb: ~A~%." limb))) - (send (send self :impedancecontrollerservice_getimpedancecontrollerparam :name sensor-name) - :i_param))) + (:raw-set-impedance-controller-param (&rest args) + (error ";; :raw-set-impedance-controller-param cannot be used with hironx~%")) + (:raw-get-impedance-controller-param (&rest args) + (error ";; :raw-get-impedance-controller-param cannot be used with hironx~%")) (:start-impedance (limb &rest args &key (m-p 100) (d-p 100) (k-p 100) (m-r 100) (d-r 2000) (k-r 2000) (ref-force #f(0 0 0)) (force-gain #f(1 1 1)) (ref-moment #f(0 0 0)) @@ -69,6 +60,10 @@ :force_gain force-gain :ref_moment ref-moment :moment_gain moment-gain :sr_gain sr-gain :avoid_gain avoid-gain :reference_gain reference-gain :manipulability_limit manipulability-limit)))) + (:raw-start-impedance (&rest args) + (error ";; :raw-start-impedance cannot be used with hironx~%")) + (:start-impedance-no-wait (&rest args) + (error ";; :start-impedance-no-wait cannot be used with hironx~%")) (:stop-impedance (limb) "Stop impedance controller mode. limb should be limb symbol name such as :rarm, :larm, or :arms." @@ -94,7 +89,28 @@ (mapcar #'(lambda (l) (send self :stop-impedance-no-wait l)) '(:rarm :larm)))) (t (error ";; No such limb: ~A~%." limb))) - (send self :impedancecontrollerservice_deleteimpedancecontroller :name sensor-name)))) + (send self :impedancecontrollerservice_deleteimpedancecontroller :name sensor-name))) + (:wait-impedance-controller-transition (&rest args) + (error ";; :wait-impedance-controller-transition cannot be used with hironx~%")) + (:set-impedance-controller-param (&rest args) + (error ";; In hironx, we cannot tell :set-impedance-controller-param from :start-impedance~%")) + (:get-impedance-controller-param (limb) + (let (sensor-name) + (cond ((eq limb :rarm) + (setq sensor-name "rhsensor")) + ((eq limb :larm) + (setq sensor-name "lhsensor")) + ((eq limb :arms) + (return-from :get-impedance-controller-param + (mapcar #'(lambda (l) (send self :get-impedance-controller-param l)) + '(:rarm :larm)))) + (t (error ";; No such limb: ~A~%." limb))) + (send (send self :impedancecontrollerservice_getimpedancecontrollerparam :name sensor-name) + :i_param))) + (:get-impedance-controller-controller-mode (&rest args) + (error ";; :get-impedance-controller-controller-mode cannot be used with hironx~%")) + (:force-sensor-method (&rest args) + (error ";; :force-sensor-method cannot be used with hironx~%"))) (defun hironx-init (&rest args) (if (not (boundp '*ri*)) From 8fbc6eddf8823b64de42affa102ddcfb5dbb4cfb Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Fri, 22 Jun 2018 01:41:47 +0900 Subject: [PATCH 7/9] Add hand interface to hironx-interface --- .../euslisp/hironx-interface.l | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l index d48c1f7f..940cb651 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l @@ -112,6 +112,62 @@ (:force-sensor-method (&rest args) (error ";; :force-sensor-method cannot be used with hironx~%"))) +;; ServoControllerService for hand +;; Based on hironx_client.py in hironx_ros_bridge and hrp2-common-interface.l +(defmethod hironx-interface + (:hand-angle-vector + (av &optional (tm 1000) (hand :hands)) + (let (av-rad-list) + ;; Convert deg float-vector (or list) to rad list + (dotimes (i (length av)) + (push (deg2rad (elt av i)) av-rad-list)) + (setq av-rad-list (reverse av-rad-list)) + (cond ((eq hand :hands) + (send self :servocontrollerservice_setjointangles :jvs av-rad-list :tm (/ tm 1000.0))) + ((or (eq hand :rhand) (eq hand :lhand)) + (send self :servocontrollerservice_setjointanglesofgroup + :gname (string-downcase hand) :jvs av-rad-list :tm (/ tm 1000.0))) + (t (error ";; No such hand: ~A~%." hand))))) + (:hand-servo-on () + (send self :servocontrollerservice_servoon)) + (:hand-servo-off () + (send self :servocontrollerservice_servooff)) + (:set-hand-effort (&optional (effort 100)) + "effort is percentage" + (dolist (id (list 2 3 4 5 6 7 8 9)) + (send self :servocontrollerservice_setmaxtorque :id id :percentage effort))) + (:hand-width2angles (width) + (let ((safetymargin 3) (l1 41.9) (l2 19) xpos a2pos a1radh a1rad) + (when (or (< width 0) (> width (* (- (+ l1 l2) safetymargin) 2))) + (return-from :hand-width2angles nil)) + (setq xpos (+ (/ width 2.0) safetymargin)) + (setq a2pos (- xpos l2)) + (setq a1radh (acos (/ a2pos l1))) + (setq a1rad (- (/ pi 2.0) a1radh)) + (float-vector a1rad (- a1rad) (- a1rad) a1rad))) + (:set-hand-width (hand width &key (tm 1000) effort) + (when effort + (send self :set-hand-effort effort)) + (send self :hand-angle-vector (send self :hand-width2angles width) tm hand)) + (:start-grasp (&optional (arm :arms) &key effort) + (cond ((eq arm :rarm) + (send self :set-hand-width :rhand 0 :effort effort)) + ((eq arm :larm) + (send self :set-hand-width :lhand 0 :effort effort)) + ((eq arm :arms) + (send self :set-hand-width :rhand 0 :effort effort) + (send self :set-hand-width :lhand 0 :effort effort)) + (t (error ";; No such arm: ~A~%." arm)))) + (:stop-grasp (&optional (arm :arms) &key effort) + (cond ((eq arm :rarm) + (send self :set-hand-width :rhand 100 :effort effort)) + ((eq arm :larm) + (send self :set-hand-width :lhand 100 :effort effort)) + ((eq arm :arms) + (send self :set-hand-width :rhand 100 :effort effort) + (send self :set-hand-width :lhand 100 :effort effort)) + (t (error ";; No such arm: ~A~%." arm))))) + (defun hironx-init (&rest args) (if (not (boundp '*ri*)) (setq *ri* (instance* hironx-interface :init args))) From 1bd3f429ab9ac99930f7a035aa23717f012cc85f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 25 Jun 2018 20:23:06 +0900 Subject: [PATCH 8/9] Fix :hand-width2angles to return deg --- hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l index 940cb651..d84dfc13 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l @@ -137,14 +137,15 @@ (dolist (id (list 2 3 4 5 6 7 8 9)) (send self :servocontrollerservice_setmaxtorque :id id :percentage effort))) (:hand-width2angles (width) - (let ((safetymargin 3) (l1 41.9) (l2 19) xpos a2pos a1radh a1rad) + (let ((safetymargin 3) (l1 41.9) (l2 19) xpos a2pos a1radh a1rad a1deg) (when (or (< width 0) (> width (* (- (+ l1 l2) safetymargin) 2))) (return-from :hand-width2angles nil)) (setq xpos (+ (/ width 2.0) safetymargin)) (setq a2pos (- xpos l2)) (setq a1radh (acos (/ a2pos l1))) (setq a1rad (- (/ pi 2.0) a1radh)) - (float-vector a1rad (- a1rad) (- a1rad) a1rad))) + (setq a1deg (rad2deg a1rad)) + (float-vector a1deg (- a1deg) (- a1deg) a1deg))) (:set-hand-width (hand width &key (tm 1000) effort) (when effort (send self :set-hand-effort effort)) From 5b345b7c3e9e8e9ed639cf4eaafb4fbe4711a8d9 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 25 Jun 2018 20:23:59 +0900 Subject: [PATCH 9/9] Add controller for each joint group --- hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l index d84dfc13..d14e85c2 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l @@ -11,8 +11,15 @@ ;; Initialize (defmethod hironx-interface + ;; Based on hrp2jsknts-interface.l (:init (&rest args) - (send-super* :init :robot hironx-robot args)) + (prog1 + (send-super* :init :robot hironx-robot args) + ;; add controller + (dolist (limb '(:rarm :larm :head :torso)) + (send self :def-limb-controller-method limb) + (send self :add-controller (read-from-string (format nil "~A-controller" limb)) + :joint-enable-check t :create-actions t)))) (:define-all-ROSBridge-srv-methods (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge")) ;; First, define ROSBridge method for old impedance controller