[W4U][hrpsys_ros_bridge_tutorials] Add Euslisp interface of HIRONXJSK supporting versioned idl

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -277,6 +277,11 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl)
     HRP3HAND_R)
 endif()
 
+# HIRONXJSK
+compile_openhrp_model_for_closed_robots(HIRONXJSK HIRONXJSK HIRONXJSK
+  --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"

hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l

@@ -0,0 +1,326 @@
+(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
+(require :hironxjsk "package://hrpsys_ros_bridge_tutorials/models/hironxjsk.l")
+(when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l"))
+  (require :hironxjsk-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l"))
+(if (ros::resolve-ros-path "package://hironx_ros_bridge")
+  (ros::load-ros-manifest "hironx_ros_bridge"))
+
+(defclass hironxjsk-interface
+  :super rtm-ros-robot-interface
+  :slots ())
+
+;; Initialize
+(defmethod hironxjsk-interface
+  ;; Based on https://github.com/start-jsk/rtmros_tutorials/blob/9132c58702b3b193e14271b4c231ad0080187850/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l
+  (:init (&rest args)
+    (prog1
+      ;; Hironx has two types of joint_states on one topic: whole body and hand,
+      ;; so queue size of joint_states should be two.
+      ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L120
+      (send-super* :init :joint-states-queue-size 2 :robot hironxjsk-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))
+      ;; Load param to erase offset of force moment sensor
+      (send self :load-forcemoment-offset-param
+            (format nil "~A/models/~A-force-moment-offset.l"
+                    (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials")
+                    (send robot :name))
+            :set-offset 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 self :define-all-ROSBridge-srv-methods-from-idl-dir
+            :ros-pkg-name "hironx_ros_bridge" :idl-dir "idl_315_1_9"))
+    ;; 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))
+  (:define-all-ROSBridge-srv-methods-from-idl-dir
+    (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge") (idl-dir "idl"))
+    (let ((srv-fnames (send self :get-ROSBridge-srv-fnames ros-pkg-name)))
+      (dolist (idl (send self :get-ROSBridge-idl-fnames-from-dir ros-pkg-name idl-dir))
+        (let ((rtc-name (pathname-name idl)))
+          (dolist (srv-name (mapcar #'pathname-name (remove-if-not #'(lambda (x) (and (substringp rtc-name x) (not (= (char x 0) (char "." 0))))) srv-fnames)))
+            (let ((method-def (send self :get-ROSBridge-method-def-macro rtc-name srv-name ros-pkg-name)))
+              (when method-def
+                (if debug-view (pprint (macroexpand method-def)))
+                (eval method-def))))))))
+  (:get-ROSBridge-fnames-from-dir-and-type
+    (dir-name type-name &optional (ros-pkg-name "hrpsys_ros_bridge"))
+    (let ((path (ros::resolve-ros-path (format nil "package://~A" ros-pkg-name))))
+      (remove-if-not #'(lambda (x) (substringp (format nil ".~A" type-name) x)) (directory (format nil "~A/~A" path dir-name)))))
+  (:get-ROSBridge-idl-fnames-from-dir
+    (&optional (ros-pkg-name "hrpsys_ros_bridge") (idl-dir "idl"))
+    (send self :get-ROSBridge-fnames-from-dir-and-type idl-dir "idl" ros-pkg-name)))
+
+;; AbsoluteForceSensor
+;; Overwrite RemoveForceSensorLinkOffset methods
+;; Based on https://github.com/start-jsk/rtmros_common/blob/1.4.2/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
+(def-set-get-param-method 'hironx_ros_bridge::OpenHRP_AbsoluteForceSensorService_ForceMomentOffsetParam
+  :raw-set-forcemoment-offset-param :raw-get-forcemoment-offset-param :get-forcemoment-offset-param-arguments
+  :absoluteforcesensorservice_setforcemomentoffsetparam :absoluteforcesensorservice_getforcemomentoffsetparam
+  :optional-args (list :name 'name))
+
+(defmethod rtm-ros-robot-interface
+  (:zero-set-forcemoment-offset-param
+   (limb)
+   "Set RemoveForceSensorLinkOffset's params offset to zero."
+   (send self :set-forcemoment-offset-param limb :force-offset #f(0 0 0) :moment-offset #f(0 0 0) :link-offset-centroid #f(0 0 0) :link-offset-mass 0)
+   )
+  (:set-forcemoment-offset-param
+   (limb &rest args)
+   "Set RemoveForceSensorLinkOffset params for given limb.
+    For arguments, please see (send *ri* :get-forcemoment-offset-param-arguments)."
+   (send* self :force-sensor-method
+         limb
+         #'(lambda (name &rest _args)
+             (send* self :raw-set-forcemoment-offset-param (send (car (send robot name :force-sensors)) :name) _args))
+         :set-forcemoment-offset-param
+         args))
+  (:get-forcemoment-offset-param
+   (limb)
+   "Get RemoveForceSensorLinkOffset params for given limb."
+   (send self :force-sensor-method
+         limb
+         #'(lambda (name &rest _args)
+             (send self :raw-get-forcemoment-offset-param (send (car (send robot name :force-sensors)) :name)))
+         :get-forcemoment-offset-param))
+  (:load-forcemoment-offset-param
+   (fname &key (set-offset t))
+   "Load AbsoluteForceSensor params from fname (file path)."
+   (mapcar #'(lambda (x)
+               (send* self :set-forcemoment-offset-param (car x)
+                      (if set-offset
+                          (cdr x)
+                        (list :link-offset-mass (cadr (memq :link-offset-mass (cdr x)))
+                              :link-offset-centroid (cadr (memq :link-offset-centroid (cdr x)))))))
+           (with-open-file
+            (f fname :direction :input)
+            (read f nil nil)))
+   )
+  (:load-forcemoment-offset-params (&rest args)
+    (error ";; :load-forcemoment-offset-params cannot be used with hironx~%"))
+  (:dump-forcemoment-offset-params (&rest args)
+    (error ";; :dump-forcemoment-offset-params cannot be used with hironx~%"))
+  (:remove-force-sensor-offset-rmfo (&rest args)
+    (error ";; :remove-force-sensor-offset-rmfo cannot be used with hironx~%"))
+  (:remove-force-sensor-offset-rmfo-arms (&rest args)
+    (error ";; :remove-force-sensor-offset-rmfo-arms cannot be used with hironx~%"))
+  (:remove-force-sensor-offset-rmfo-legs (&rest args)
+    (error ";; :remove-force-sensor-offset-rmfo-legs cannot be used with hironx~%"))
+  ;; Deprecated in https://github.com/start-jsk/rtmros_common/pull/1010,
+  ;; but new methods cannot be used in hironx.
+  ;; So we revert deprecated methods to the ones before the PR and use them.
+  (:reset-force-moment-offset-arms
+   ()
+   "Remove force and moment offset for :rarm and :larm"
+   (send self :reset-force-moment-offset '(:rarm :larm)))
+  (:reset-force-moment-offset-legs (&rest args)
+    (error ";; :reset-force-moment-offset-legs cannot be used with hironx~%"))
+  (:reset-force-moment-offset
+   (limbs)
+   "Remove force and moment offsets. limbs should be list of limb symbol name."
+   (send self :_reset-force-moment-offset limbs :force)
+   (send self :_reset-force-moment-offset limbs :moment)
+   )
+  (:_reset-force-moment-offset
+   (limbs f/m &key (itr 10))
+   (let* ((params (mapcar #'(lambda (alimb) (send self :get-forcemoment-offset-param alimb)) limbs)))
+     (labels ((calc-off
+               (alimb)
+               (send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb))
+              (get-avg-fm
+               ()
+               (let ((fm (mapcar #'(lambda (i)
+                                     (send self :state)
+                                     (mapcar #'(lambda (alimb) (send self (if (eq f/m :force) :off-force-vector :off-moment-vector) alimb)) limbs))
+                                 (make-list itr))))
+                 (mapcar #'(lambda (alimb)
+                             (let ((idx (position alimb limbs)))
+                               (vector-mean (mapcar #'(lambda (d) (elt d idx)) fm))))
+                         limbs))))
+       ;; estimate offsets
+       (let* ((tmp-fm-offsets (mapcar #'(lambda (i)
+                                          (send self :state)
+                                          (mapcar #'calc-off limbs))
+                                      (make-list itr)))
+              (new-fm-offsets (mapcar #'(lambda (alimb)
+                                          (let ((idx (position alimb limbs)))
+                                            (vector-mean (mapcar #'(lambda (d) (elt d idx)) tmp-fm-offsets))))
+                                      limbs))
+              (org-fm-list (get-avg-fm)))
+         ;; set offsets
+         (mapcar #'(lambda (alimb new-fm-offset param)
+                     (send self :set-forcemoment-offset-param alimb
+                           (if (eq f/m :force) :force-offset :moment-offset)
+                           (v+ (if (eq f/m :force)
+                                   (send param :force_offset)
+                                 (send param :moment_offset))
+                               new-fm-offset)))
+                 limbs new-fm-offsets params)
+         (unix:usleep 10000)
+         ;; check ;; compare sensor value before & after resetting
+         (mapcar #'(lambda (alimb org-fm new-fm)
+                     (format t ";; ~A error of ~A ;; ~A[~A] -> ~A[~A]~%"
+                             (string-downcase f/m) alimb
+                             (norm org-fm) (if (eq f/m :force) "N" "Nm")
+                             (norm new-fm) (if (eq f/m :force) "N" "Nm")))
+                 limbs org-fm-list (get-avg-fm))
+         ))))
+  )
+
+;; ImpedanceControllerService
+;; Based on https://github.com/start-jsk/rtmros_hironx/blob/2.1.0/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
+;; and https://github.com/start-jsk/rtmros_common/blob/1.4.2/hrpsys_ros_bridge/euslisp/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 hironxjsk-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~%")))
+
+;; ServoControllerService for hand
+;; Based on https://github.com/start-jsk/rtmros_hironx/blob/2.1.0/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
+;; and https://github.com/start-jsk/rtmros_tutorials/blob/0.1.6/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l
+(defmethod hironxjsk-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 hironxjsk-init (&rest args)
+  (if (not (boundp '*ri*))
+      (setq *ri* (instance* hironxjsk-interface :init args)))
+  (if (not (boundp '*hironxjsk*))
+      (setq *hironxjsk* (instance hironxjsk-robot :init))))

hrpsys_ros_bridge_tutorials/models/HIRONXJSK-force-moment-offset.l

@@ -0,0 +1 @@
+((:rarm :force-offset #f(2.42349 2.03268 0.237616) :moment-offset #f(0.590862 -0.944791 -0.01666) :link-offset-mass 0.377791 :link-offset-centroid #f(-0.007461 -0.021374 -0.235109)) (:larm :force-offset #f(-2.6364 2.73913 0.714611) :moment-offset #f(0.632441 0.523776 0.097475) :link-offset-mass 0.342879 :link-offset-centroid #f(-0.002918 0.00508 -0.233272)))

hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml

@@ -0,0 +1,45 @@
+##
+## - 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]
+  ## _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]