[hrpsys_ros_bridge_tutorials] Add Euslisp interface of HIRONX supporting impedance and hand

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 "")

hrpsys_ros_bridge_tutorials/euslisp/hironx-interface.l

@@ -0,0 +1,183 @@
+(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
+  ;; Based on hrp2jsknts-interface.l
+  (:init (&rest 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
+    (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.
+;; 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.
+(defmethod hironx-interface
+  (: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))
+          (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))))
+  (: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."
+    (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)))
+  (: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~%")))
+
+;; 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 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))
+      (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))
+    (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)))
+  (if (not (boundp '*hironx*))
+      (setq *hironx* (instance hironx-robot :init))))

hrpsys_ros_bridge_tutorials/models/kawada-hironx.yaml → 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]