[hrpsys_ros_bridge_tutorials] Euslisp interface of HIRONXJSK for latest hrpsys

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -277,6 +277,28 @@ 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-dt-option "0.005"
+  --conf-file-option "# This conf should be backup of /opt/jsk/etc/HIRONX/hrprtc/Robot.conf inside HIRONXJSK"
+  --conf-file-option "# model should be /opt/jsk/etc/HIRONX/model/main.wrl inside HIRONXJSK"
+  --conf-file-option "pdgains.file_name: /opt/jsk/etc/HIRONX/hrprtc/PDgains.sav"
+  --conf-file-option "naming.formats: %n.rtc"
+  --conf-file-option "# RTC disable/enable settings which will be loaded via RobotHardware"
+  --conf-file-option "CollisionDetector.enable: YES"
+  --conf-file-option "# for HIRO hand servo"
+  --conf-file-option "servo.id: 2,3,4,5, 6,7,8,9"
+  --conf-file-option "servo.offset: -0.80,0.0,-0.85,0.0, -0.85,0.0,-0.89,0.0"
+  --conf-file-option "servo.devname: /dev/serusb1"
+  --conf-file-option "servo.dir 1,1,-1,-1,1,1,-1,-1"
+  --conf-file-option "# for RemoveForceSensorLinkOffset"
+  --conf-file-option "forcemoment_offset_params: /opt/jsk/etc/HIRONX/hrprtc/force_sensor_calib_20191127"
+  --conf-file-option "# for ImpedanceController"
+  --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,"
+  --conf-file-option "# for CollisionDetector"
+  --conf-file-option "collision_pair: HEAD_JOINT0:RARM_JOINT2 HEAD_JOINT0:RARM_JOINT3 HEAD_JOINT0:RARM_JOINT4 HEAD_JOINT0:RARM_JOINT5 HEAD_JOINT0:LARM_JOINT2 HEAD_JOINT0:LARM_JOINT3 HEAD_JOINT0:LARM_JOINT4 HEAD_JOINT0:LARM_JOINT5 HEAD_JOINT1:RARM_JOINT2 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:LARM_JOINT2 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 WAIST:RARM_JOINT3 WAIST:RARM_JOINT4 WAIST:RARM_JOINT5 WAIST:LARM_JOINT3 WAIST:LARM_JOINT4 WAIST:LARM_JOINT5 CHEST_JOINT0:RARM_JOINT2 CHEST_JOINT0:RARM_JOINT3 CHEST_JOINT0:RARM_JOINT4 CHEST_JOINT0:RARM_JOINT5 CHEST_JOINT0:LARM_JOINT2 CHEST_JOINT0:LARM_JOINT3 CHEST_JOINT0:LARM_JOINT4 CHEST_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT1 RARM_JOINT0:LARM_JOINT2 RARM_JOINT0:LARM_JOINT3 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT1:LARM_JOINT0 RARM_JOINT1:LARM_JOINT1 RARM_JOINT1:LARM_JOINT2 RARM_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT2:LARM_JOINT0 RARM_JOINT2:LARM_JOINT1 RARM_JOINT2:LARM_JOINT2 RARM_JOINT2:LARM_JOINT3 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT3:LARM_JOINT0 RARM_JOINT3:LARM_JOINT1 RARM_JOINT3:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5"
+  )
+
 # 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,189 @@
+(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"))
+
+(defclass hironxjsk-interface
+  :super rtm-ros-robot-interface
+  :slots (hand-servo-num))
+
+;; 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))
+      ;; number of servo motors in one hand
+      (setq hand-servo-num 4)))
+  (:call-operation-return (method &rest args)
+    ;; Call method until it returns true
+    ;; Used to ensure operation on the hand service calls, that sometimes fail
+    (do ((res (send* self method args)
+              (send* self method args)))
+        ((send res :operation_return) res))))
+
+;; 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
+  (:check-hand-vector-length (vec &optional (hand-num 1))
+   ;; Ensure that `vec' is a 4 element vector for single hand or 8 element for double hand
+   (let ((len (* hand-num hand-servo-num)))
+     (assert (= (length vec) len)
+             "[ERROR] Expecting vector of length ~a~%" len)))
+  (:hand-angle-vector (hand &optional av (tm 1000))
+   (when av
+     ;; check type
+     (case hand
+           (:hands
+            (if (= (length av) hand-servo-num) (setq av (concatenate float-vector av av)))
+            (send self :check-hand-vector-length av 2))
+           ((:rhand :lhand)
+            (send self :check-hand-vector-length av))))
+
+   ;; simulation mode
+   (when (send self :simulation-modep)
+     (flet ((get-joint-list (hand)
+              (let (acc)
+                (dotimes (i 4) (push (read-from-string (format nil "~a_joint~a" hand i)) acc))
+                (nreverse acc))))
+       (let ((joint-list (case hand
+                           (:hands (append (get-joint-list :rhand) (get-joint-list :lhand)))
+                           ((:rhand :lhand) (get-joint-list hand))
+                           (t (error ";; No such hand: ~A~%." hand)))))
+         (return-from :hand-angle-vector
+           (if av
+             ;; setjointangles
+             (map nil #'(lambda (joint angle) (send robot joint :joint-angle angle))
+                   joint-list av)
+             ;; getjointangles
+             (map float-vector #'(lambda (joint) (send robot joint :joint-angle))
+                  joint-list))))))
+   ;; real robot
+   (if av
+     ;; setjointangles
+     (let ((av-rad-list (map cons #'deg2rad av)))
+       (case hand
+             (:hands
+              (send self :call-operation-return :servocontrollerservice_setjointangles
+                           :jvs av-rad-list :tm (/ tm 1000.0)))
+             ((:rhand :lhand)
+              (send self :call-operation-return :servocontrollerservice_setjointanglesofgroup
+                    :gname (string-downcase hand) :jvs av-rad-list :tm (/ tm 1000.0)))
+             (t (error ";; No such hand: ~A~%." hand))))
+     ;; getjointangles
+     (let ((ids (case hand
+                      (:hands (list 2 3 4 5 6 7 8 9))
+                      (:rhand (list 2 3 4 5))
+                      (:lhand (list 6 7 8 9))
+                      (t (error ";; No such hand: ~A~%." hand))))
+           (dirs (case hand
+                       (:hands #f(1 1 -1 -1 1 1 -1 -1))
+                       ((:lhand :rhand) #f(1 1 -1 -1)))))
+       ;; servocontrollerservice_getjointangles do not consider servo offset and direction
+       ;; servocontrollerservice_getjointangle do not consider servo direction
+       ;; defined in /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
+       ;; servo.id: 2,3,4,5, 6,7,8,9
+       ;; servo.offset: -0.78,0.0,-0.82,0.0, -0.85,0.0,-0.82,0.0
+       ;; servo.dir 1,1,-1,-1,1,1,-1,-1
+       (map float-vector
+            #'(lambda (id dir)
+                (* dir (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv)))
+            ids dirs))))
+  (:hand-servo-on ()
+    (unless (send self :simulation-modep)
+      (send self :call-operation-return :servocontrollerservice_servoon)))
+  (:hand-servo-off ()
+    (unless (send self :simulation-modep)
+      (send self :call-operation-return :servocontrollerservice_servooff)))
+  (:hand-effort (&optional (hand :hands) effort)
+    ;; effort is percentage or sequence of percentages
+    (if (send self :simulation-modep) (return-from :hand-effort nil))
+    (let ((ids (case hand
+                     (:hands (list 2 3 4 5 6 7 8 9))
+                     (:rhand (list 2 3 4 5))
+                     (:lhand (list 6 7 8 9))
+                     (t (error ";; No such hand: ~A~%." hand)))))
+      (cond
+       ((null effort)
+        ;; getmaxtorque
+        (mapcar
+         #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
+         ids))
+       ((and (numberp effort) (plusp effort))
+        ;; setmaxtorque with same effort value
+        (mapcar
+         #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
+         ids))
+       ((or (consp effort) (vectorp effort))
+        ;; check length
+        (case hand
+              (:hands
+               (if (= (length effort) hand-servo-num)
+                   (setq effort (concatenate float-vector effort effort)))
+               (send self :check-hand-vector-length effort 2))
+              ((:rhand :lhand)
+               (send self :check-hand-vector-length effort)))
+        ;; setmaxtorque with different effort values
+        (map cons
+         #'(lambda (id val)
+             (if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val)))
+         ids effort))
+       (t
+        ;; unsupported type
+        (error "number or sequence expected")))))
+  (:hand-width2angles (width)
+    ;; Calculates the hand angles to achieve a certain parallel aperture
+    (let ((safetymargin 3) (w0 19) (l1 41.9))
+      (unless (<= (- safetymargin) width %(2 * (w0 + l1 - safetymargin)))
+        (warn ";; width value ~a is off margins~%" width)
+        (return-from :hand-width2angles nil))
+      (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
+        (float-vector a (- a) (- a) a))))
+  (:hand-angles2width (vec)
+    ;; Calculates the hand aperture given a certain angle vector
+    (send self :check-hand-vector-length vec)
+    (let ((safetymargin 3) (w0 19) (l1 41.9) (l2 20))
+      (flet ((get-width (r1 r2) %( w0 + l1 * cos(pi/2 - r1) + l2 * cos(pi/2 - r1 - r2) - safetymargin)))
+            (multiple-value-bind (a1 a2 b1 b2) (map cons #'deg2rad vec)
+              (+ (get-width a1 a2)
+                 (get-width (- b1) (- b2)))))))
+  (:hand-width (hand &optional width &key (time 1000) effort)
+    ;; Get/Set the hand width
+    (if width
+      ;; set hand width
+      (progn
+        (when effort (send self :hand-effort hand effort))
+        (send self :hand-angle-vector hand (send self :hand-width2angles width) time))
+      ;; get hand width
+      (send self :hand-angles2width (send self :hand-angle-vector hand))))
+  (:start-grasp (&optional (arm :arms) &key (time 1000) effort)
+    (case arm
+          (:arms (setq arm :hands))
+          (:rarm (setq arm :rhand))
+          (:larm (setq arm :lhand)))
+    (send self :hand-width arm 0 :time time :effort effort))
+  (:stop-grasp (&optional (arm :arms) &key (time 1000) effort)
+    (case arm
+          (:arms (setq arm :hands))
+          (:rarm (setq arm :rhand))
+          (:larm (setq arm :lhand)))
+    (send self :hand-width arm 100 :time time :effort effort)))
+
+(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)))
+  ;; read initial robot state
+  (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector))
+  ;; return robot instance
+  *hironxjsk*)

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]