From fb2b1e7bb803dbb6c9c430f2d319d11d9632d90b Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 26 Jun 2018 19:19:42 +0900
Subject: [PATCH 01/23] Auto-generate Euslisp model of HIRONXJSK

---
 hrpsys_ros_bridge_tutorials/CMakeLists.txt    |  5 +++
 .../models/hironxjsk.yaml                     | 45 +++++++++++++++++++
 2 files changed, 50 insertions(+)
 create mode 100644 hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml

diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt
index 5e2b7745..94998f6a 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()
 
+# 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"
diff --git a/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml b/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml
new file mode 100644
index 00000000..e0e6d122
--- /dev/null
+++ b/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]

From 0c84143b3aae7ad7f4d6daa0780170dd1ad17226 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 26 Jun 2018 19:20:59 +0900
Subject: [PATCH 02/23] Add hironxjsk-interface supporting impedance and hand

---
 .../euslisp/hironxjsk-interface.l             | 183 ++++++++++++++++++
 1 file changed, 183 insertions(+)
 create mode 100644 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
new file mode 100644
index 00000000..d9605f97
--- /dev/null
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -0,0 +1,183 @@
+(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 hrp2jsknts-interface.l
+  (:init (&rest args)
+    (prog1
+      (send-super* :init :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))))
+  (: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 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~%"))
+  (: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 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))))

From c199fec8e21b203be2ddcf693eae469c8df33a1e Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 29 Jun 2018 21:02:39 +0900
Subject: [PATCH 03/23] Overwrite RemoveForceSensorLinkOffset methods for
 AbsoluteForceSensor

---
 .../euslisp/hironxjsk-interface.l             | 119 +++++++++++++++++-
 1 file changed, 116 insertions(+), 3 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index d9605f97..b7243187 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -30,6 +30,121 @@
     ;; See :get-ROSBridge-method-def-macro
     (send-super :define-all-ROSBridge-srv-methods)))
 
+;; AbsoluteForceSensor
+;; Overwrite RemoveForceSensorLinkOffset methods
+(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 hironx_client.py in hironx_ros_bridge and rtm-ros-robot-interface.l.
 ;; Enable methods executable with old impedance controller and disable others.
@@ -115,9 +230,7 @@
       (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~%")))
+    (error ";; :get-impedance-controller-controller-mode cannot be used with hironx~%")))
 
 ;; ServoControllerService for hand
 ;; Based on hironx_client.py in hironx_ros_bridge and hrp2-common-interface.l

From 96abbac9b72a3c4dc391697ed02cba1d643b95e7 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 30 Jun 2018 17:09:08 +0900
Subject: [PATCH 04/23] Indicate version of base code

---
 .../euslisp/hironxjsk-interface.l                        | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index b7243187..7eefacb7 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -11,7 +11,7 @@
 
 ;; Initialize
 (defmethod hironxjsk-interface
-  ;; Based on hrp2jsknts-interface.l
+  ;; Based on https://github.com/start-jsk/rtmros_tutorials/blob/9132c58702b3b193e14271b4c231ad0080187850/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l
   (:init (&rest args)
     (prog1
       (send-super* :init :robot hironxjsk-robot args)
@@ -32,6 +32,7 @@
 
 ;; 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
@@ -146,7 +147,8 @@
   )
 
 ;; ImpedanceControllerService
-;; Based on hironx_client.py in hironx_ros_bridge and rtm-ros-robot-interface.l.
+;; 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,
@@ -233,7 +235,8 @@
     (error ";; :get-impedance-controller-controller-mode cannot be used with hironx~%")))
 
 ;; ServoControllerService for hand
-;; Based on hironx_client.py in hironx_ros_bridge and hrp2-common-interface.l
+;; 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))

From dcfaf4427fe766b4b4a6a4606413199c7bbaf13c Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 2 Jul 2018 17:12:47 +0900
Subject: [PATCH 05/23] Erase offset of force moment sensor of HIRONXJSK

Old hrpsys in HIRONX itself has no way to load file of force moment offset param, so we do that with Euslisp
---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 8 +++++++-
 .../models/HIRONXJSK-force-moment-offset.l                | 1 +
 2 files changed, 8 insertions(+), 1 deletion(-)
 create mode 100644 hrpsys_ros_bridge_tutorials/models/HIRONXJSK-force-moment-offset.l

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 7eefacb7..714aa49d 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -19,7 +19,13 @@
       (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))))
+              :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
diff --git a/hrpsys_ros_bridge_tutorials/models/HIRONXJSK-force-moment-offset.l b/hrpsys_ros_bridge_tutorials/models/HIRONXJSK-force-moment-offset.l
new file mode 100644
index 00000000..30ee4a77
--- /dev/null
+++ b/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)))

From 2de9bdad74a2d330849f66737338a1104e24c42d Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 9 Jul 2018 16:03:45 +0900
Subject: [PATCH 06/23] Set queue size of joint_states as 2

Because hironx has two types of joint_states on one topic: whole body and hand
---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 714aa49d..7d748ba9 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -14,7 +14,10 @@
   ;; Based on https://github.com/start-jsk/rtmros_tutorials/blob/9132c58702b3b193e14271b4c231ad0080187850/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l
   (:init (&rest args)
     (prog1
-      (send-super* :init :robot hironxjsk-robot args)
+      ;; 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)

From 630bed101172b60e9faa4e37d071804c145beda2 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Fri, 30 Nov 2018 12:22:41 +0900
Subject: [PATCH 07/23] Change 'set-hand-effort' to 'hand-effort'

---
 .../euslisp/hironxjsk-interface.l                | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 7d748ba9..4d6e1c6d 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -264,10 +264,18 @@
     (send self :servocontrollerservice_servoon))
   (:hand-servo-off ()
     (send self :servocontrollerservice_servooff))
-  (:set-hand-effort (&optional (effort 100))
+  (:hand-effort (&optional (hand :hands) effort)
     "effort is percentage"
-    (dolist (id (list 2 3 4 5 6 7 8 9))
-      (send self :servocontrollerservice_setmaxtorque :id id :percentage effort)))
+    (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)))))
+      (if effort
+        ;; setmaxtorque
+        (mapcar #'(lambda (id) (send self :servocontrollerservice_setmaxtorque :id id :percentage effort)) ids)
+        ;; getmaxtorque
+        (mapcar #'(lambda (id) (send (send self :servocontrollerservice_getmaxtorque :id id) :percentage)) ids))))
   (: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)))
@@ -280,7 +288,7 @@
       (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-effort hand effort))
     (send self :hand-angle-vector (send self :hand-width2angles width) tm hand))
   (:start-grasp (&optional (arm :arms) &key effort)
     (cond ((eq arm :rarm)

From 2b92b80bc8793f4cfbb096c7ab4c131140a0ab91 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Fri, 30 Nov 2018 13:25:35 +0900
Subject: [PATCH 08/23] Add getter to 'hand-angle-vector'

---
 .../euslisp/hironxjsk-interface.l             | 32 +++++++++++--------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 4d6e1c6d..a3355c84 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -248,18 +248,24 @@
 ;; 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)))))
+   (&optional (hand :hands) av (tm 1000))
+   (if av
+     ;; setjointangles
+     (let ((av-rad-list (map cons #'deg2rad av)))
+       (case hand
+             (:hands (send self :servocontrollerservice_setjointangles :jvs av-rad-list :tm (/ tm 1000.0)))
+             ((:rhand :lhand)
+              (send self :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)))))
+       ;; servocontroller_services_getjointangles do not consider servo offset
+       (map float-vector #'(lambda (id) (send (send self :servocontrollerservice_getjointangle :id id) :jv)) ids))))
   (:hand-servo-on ()
     (send self :servocontrollerservice_servoon))
   (:hand-servo-off ()
@@ -289,7 +295,7 @@
   (:set-hand-width (hand width &key (tm 1000) effort)
     (when effort
       (send self :hand-effort hand effort))
-    (send self :hand-angle-vector (send self :hand-width2angles width) tm hand))
+    (send self :hand-angle-vector hand (send self :hand-width2angles width) tm))
   (:start-grasp (&optional (arm :arms) &key effort)
     (cond ((eq arm :rarm)
            (send self :set-hand-width :rhand 0 :effort effort))

From b7ad292d81e461f1547c481b3e4d9cb7ada270fd Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Fri, 30 Nov 2018 13:44:07 +0900
Subject: [PATCH 09/23] Ensure 'operation_return' on service call

---
 .../euslisp/hironxjsk-interface.l             | 29 +++++++++++++------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index a3355c84..bb2c8a6a 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -37,7 +37,11 @@
     ;; 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)))
+    (send-super :define-all-ROSBridge-srv-methods))
+  (:call-operation-return (method &rest args)
+    (do ((res (send* self method args)
+              (send* self method args)))
+        ((send res :operation_return) res))))
 
 ;; AbsoluteForceSensor
 ;; Overwrite RemoveForceSensorLinkOffset methods
@@ -253,10 +257,11 @@
      ;; setjointangles
      (let ((av-rad-list (map cons #'deg2rad av)))
        (case hand
-             (:hands (send self :servocontrollerservice_setjointangles :jvs av-rad-list :tm (/ tm 1000.0)))
+             (:hands (send self :call-operation-return :servocontrollerservice_setjointangles
+                           :jvs av-rad-list :tm (/ tm 1000.0)))
              ((:rhand :lhand)
-              (send self :servocontrollerservice_setjointanglesofgroup :gname (string-downcase hand)
-                    :jvs av-rad-list :tm (/ tm 1000.0)))
+              (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
@@ -265,11 +270,13 @@
                       (:lhand (list 6 7 8 9))
                       (t (error ";; No such hand: ~A~%." hand)))))
        ;; servocontroller_services_getjointangles do not consider servo offset
-       (map float-vector #'(lambda (id) (send (send self :servocontrollerservice_getjointangle :id id) :jv)) ids))))
+       (map float-vector
+            #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv))
+            ids))))
   (:hand-servo-on ()
-    (send self :servocontrollerservice_servoon))
+    (send self :call-operation-return :servocontrollerservice_servoon))
   (:hand-servo-off ()
-    (send self :servocontrollerservice_servooff))
+    (send self :call-operation-return :servocontrollerservice_servooff))
   (:hand-effort (&optional (hand :hands) effort)
     "effort is percentage"
     (let ((ids (case hand
@@ -279,9 +286,13 @@
                      (t (error ";; No such hand: ~A~%." hand)))))
       (if effort
         ;; setmaxtorque
-        (mapcar #'(lambda (id) (send self :servocontrollerservice_setmaxtorque :id id :percentage effort)) ids)
+        (mapcar
+         #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
+         ids)
         ;; getmaxtorque
-        (mapcar #'(lambda (id) (send (send self :servocontrollerservice_getmaxtorque :id id) :percentage)) ids))))
+        (mapcar
+         #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
+         ids))))
   (: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)))

From e3b5c72dccedfb93fbe1fc87d678cffe37d00a9a Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Fri, 30 Nov 2018 14:10:21 +0900
Subject: [PATCH 10/23] Fix servo direction

---
 .../euslisp/hironxjsk-interface.l               | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index bb2c8a6a..7bc113b3 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -268,11 +268,20 @@
                       (: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)))))
-       ;; servocontroller_services_getjointangles do not consider servo offset
+                      (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)))))
+       ;; servocontroller_services_getjointangles do not consider servo offset and direction
+       ;; servocontroller_services_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) (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv))
-            ids))))
+            #'(lambda (id dir)
+                (* dir (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv)))
+            ids dirs))))
   (:hand-servo-on ()
     (send self :call-operation-return :servocontrollerservice_servoon))
   (:hand-servo-off ()

From c88e5a58568fa5dc692af378bd662066c4496bc0 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Fri, 30 Nov 2018 17:08:59 +0900
Subject: [PATCH 11/23] Fix comment

---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 7bc113b3..0239b45b 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -272,8 +272,8 @@
            (dirs (case hand
                        (:hands #f(1 1 -1 -1 1 1 -1 -1))
                        ((:lhand :rhand) #f(1 1 -1 -1)))))
-       ;; servocontroller_services_getjointangles do not consider servo offset and direction
-       ;; servocontroller_services_getjointangle do not consider servo direction
+       ;; 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

From 159fd554fd262b1fa3558ce77afddc9372e9f5ce Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Mon, 3 Dec 2018 18:36:16 +0900
Subject: [PATCH 12/23] Change 'set-hand-width' to 'hand-width'

---
 .../euslisp/hironxjsk-interface.l             | 34 ++++++++++++-------
 1 file changed, 21 insertions(+), 13 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 0239b45b..e81e5aa9 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -252,7 +252,7 @@
 ;; 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
-   (&optional (hand :hands) av (tm 1000))
+   (hand &optional av (tm 1000))
    (if av
      ;; setjointangles
      (let ((av-rad-list (map cons #'deg2rad av)))
@@ -303,19 +303,27 @@
          #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
          ids))))
   (: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)))
+    (let ((safetymargin 3) (w0 19) (l1 41.9))
+      (unless (<= 0 width %(2 * (w0 + l1 - safetymargin)))
+        (warn ";; width value ~a is off margins~%" width)
         (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 :hand-effort hand effort))
-    (send self :hand-angle-vector hand (send self :hand-width2angles width) tm))
+      (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
+        (float-vector a (- a) (- a) a))))
+  (:hand-angles2width (vec)
+    (assert (= (length vec) 4))
+    (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 (tm 1000) effort)
+    (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) tm))
+      ;; get hand width
+      (send self :hand-angles2width (send self :hand-angle-vector hand))))
   (:start-grasp (&optional (arm :arms) &key effort)
     (cond ((eq arm :rarm)
            (send self :set-hand-width :rhand 0 :effort effort))

From a58e77e34f53908fe5995f3fbcbc1f8c0b6ce33e Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Mon, 3 Dec 2018 19:28:54 +0900
Subject: [PATCH 13/23] Replace set-hand-width

---
 .../euslisp/hironxjsk-interface.l                | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index e81e5aa9..c5c723a1 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -326,21 +326,21 @@
       (send self :hand-angles2width (send self :hand-angle-vector hand))))
   (:start-grasp (&optional (arm :arms) &key effort)
     (cond ((eq arm :rarm)
-           (send self :set-hand-width :rhand 0 :effort effort))
+           (send self :hand-width :rhand 0 :effort effort))
           ((eq arm :larm)
-           (send self :set-hand-width :lhand 0 :effort effort))
+           (send self :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))
+           (send self :hand-width :rhand 0 :effort effort)
+           (send self :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))
+           (send self :hand-width :rhand 100 :effort effort))
           ((eq arm :larm)
-           (send self :set-hand-width :lhand 100 :effort effort))
+           (send self :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))
+           (send self :hand-width :rhand 100 :effort effort)
+           (send self :hand-width :lhand 100 :effort effort))
           (t (error ";; No such arm: ~A~%." arm)))))
 
 (defun hironxjsk-init (&rest args)

From 658728170363887a4d7efcd2496bf62b61fa0a89 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Tue, 4 Dec 2018 14:12:32 +0900
Subject: [PATCH 14/23] Read robot state on hironxjsk-init

---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index c5c723a1..d881f984 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -347,4 +347,8 @@
   (if (not (boundp '*ri*))
       (setq *ri* (instance* hironxjsk-interface :init args)))
   (if (not (boundp '*hironxjsk*))
-      (setq *hironxjsk* (instance hironxjsk-robot :init))))
+      (setq *hironxjsk* (instance hironxjsk-robot :init)))
+  ;; read initial robot state
+  (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector))
+  ;; return robot instance
+  *hironxjsk*)

From 2b24963b322d0f2393c41a15d3d347656c3e391a Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Tue, 4 Dec 2018 14:47:25 +0900
Subject: [PATCH 15/23] Add :time key to :start-grasp

---
 .../euslisp/hironxjsk-interface.l             | 23 +++++++++----------
 1 file changed, 11 insertions(+), 12 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index d881f984..f2b5b3e5 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -257,7 +257,9 @@
      ;; setjointangles
      (let ((av-rad-list (map cons #'deg2rad av)))
        (case hand
-             (:hands (send self :call-operation-return :servocontrollerservice_setjointangles
+             (:hands
+              (when (= (length av) 4) (setq av-rad-list (concatenate float-vector av-rad-list av-rad-list)))
+              (send self :call-operation-return :servocontrollerservice_setjointangles
                            :jvs av-rad-list :tm (/ tm 1000.0)))
              ((:rhand :lhand)
               (send self :call-operation-return :servocontrollerservice_setjointanglesofgroup
@@ -316,23 +318,20 @@
             (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 (tm 1000) effort)
+  (:hand-width (hand &optional width &key (time 1000) effort)
     (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) tm))
+        (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 effort)
-    (cond ((eq arm :rarm)
-           (send self :hand-width :rhand 0 :effort effort))
-          ((eq arm :larm)
-           (send self :hand-width :lhand 0 :effort effort))
-          ((eq arm :arms)
-           (send self :hand-width :rhand 0 :effort effort)
-           (send self :hand-width :lhand 0 :effort effort))
-          (t (error ";; No such arm: ~A~%." arm))))
+  (: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 effort)
     (cond ((eq arm :rarm)
            (send self :hand-width :rhand 100 :effort effort))

From d554b835ba9c8722250ff6b032b06392500cf9a7 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Tue, 4 Dec 2018 15:04:16 +0900
Subject: [PATCH 16/23] Add :time key to :stop-grasp

---
 .../euslisp/hironxjsk-interface.l                 | 15 ++++++---------
 1 file changed, 6 insertions(+), 9 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index f2b5b3e5..03a4b2f9 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -332,15 +332,12 @@
           (:rarm (setq arm :rhand))
           (:larm (setq arm :lhand)))
     (send self :hand-width arm 0 :time time :effort effort))
-  (:stop-grasp (&optional (arm :arms) &key effort)
-    (cond ((eq arm :rarm)
-           (send self :hand-width :rhand 100 :effort effort))
-          ((eq arm :larm)
-           (send self :hand-width :lhand 100 :effort effort))
-          ((eq arm :arms)
-           (send self :hand-width :rhand 100 :effort effort)
-           (send self :hand-width :lhand 100 :effort effort))
-          (t (error ";; No such arm: ~A~%." arm)))))
+  (: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*))

From 18acc771bbcbf9a370970a6f5aa174272f8b6bd4 Mon Sep 17 00:00:00 2001
From: Guilherme <guilherme.c.affonso@gmail.com>
Date: Tue, 4 Dec 2018 19:32:56 +0900
Subject: [PATCH 17/23] Add assertation message

---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 03a4b2f9..e03b5094 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -312,7 +312,7 @@
       (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
         (float-vector a (- a) (- a) a))))
   (:hand-angles2width (vec)
-    (assert (= (length vec) 4))
+    (assert (= (length vec) 4) "Expecting vector of length 4~%")
     (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)

From 4b003599d6ae9d6358640ce8a83194475f7ff7ef Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Wed, 5 Dec 2018 17:04:54 +0900
Subject: [PATCH 18/23] Allow sequence input on :hand-effort

---
 .../euslisp/hironxjsk-interface.l             | 20 +++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index e03b5094..ebe32fe4 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -289,21 +289,29 @@
   (:hand-servo-off ()
     (send self :call-operation-return :servocontrollerservice_servooff))
   (:hand-effort (&optional (hand :hands) effort)
-    "effort is percentage"
+    "effort is percentage or sequence of percentages"
     (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)))))
-      (if effort
-        ;; setmaxtorque
+      (cond
+       ((numberp effort)
+        ;; setmaxtorque with same effort value
         (mapcar
          #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort))
-         ids)
-        ;; getmaxtorque
+         ids))
+       ;; setmaxtorque with different effort values
+       (effort
+        (map cons
+         #'(lambda (id val)
+             (if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val)))
+         ids effort))
+       ;; getmaxtorque
+       (t
         (mapcar
          #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
-         ids))))
+         ids)))))
   (:hand-width2angles (width)
     (let ((safetymargin 3) (w0 19) (l1 41.9))
       (unless (<= 0 width %(2 * (w0 + l1 - safetymargin)))

From 488b4e43da37629f24242f0ada83425b3f01bdae Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Thu, 20 Dec 2018 21:29:39 +0900
Subject: [PATCH 19/23] Do not load force-moment offset in simulation mode

---
 .../euslisp/hironxjsk-interface.l                     | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index ebe32fe4..a0561444 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -24,11 +24,12 @@
         (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)))
+      (unless (send self :simulation-modep)
+        (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

From 6aa2a4f0eee3f57d5095c87928387043612fd32f Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Thu, 20 Dec 2018 22:40:43 +0900
Subject: [PATCH 20/23] Provide hand functions on simulation mode

---
 .../euslisp/hironxjsk-interface.l             | 28 +++++++++++++++++--
 1 file changed, 26 insertions(+), 2 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index a0561444..533a90a3 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -254,6 +254,27 @@
 (defmethod hironxjsk-interface
   (:hand-angle-vector
    (hand &optional av (tm 1000))
+   ;; 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)))))
+         (if (and (eql hand :hands) (= (length av) 4))
+             (setq av (concatenate float-vector av av)))
+         (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)))
@@ -286,11 +307,14 @@
                 (* dir (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv)))
             ids dirs))))
   (:hand-servo-on ()
-    (send self :call-operation-return :servocontrollerservice_servoon))
+    (unless (send self :simulation-modep)
+      (send self :call-operation-return :servocontrollerservice_servoon)))
   (:hand-servo-off ()
-    (send self :call-operation-return :servocontrollerservice_servooff))
+    (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))

From 6496edcbaea300342b15f0cd48f9473e9864620f Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Fri, 21 Dec 2018 10:09:02 +0900
Subject: [PATCH 21/23] Allow ignoring safetymargin on hand-width

---
 hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 533a90a3..189f9e6b 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -339,7 +339,7 @@
          ids)))))
   (:hand-width2angles (width)
     (let ((safetymargin 3) (w0 19) (l1 41.9))
-      (unless (<= 0 width %(2 * (w0 + l1 - safetymargin)))
+      (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)))))

From d66f87fa0be235eb94b6ef998e2d75874c9aa238 Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Wed, 3 Apr 2019 18:25:32 +0900
Subject: [PATCH 22/23] Add typecheck to hand functions

---
 .../euslisp/hironxjsk-interface.l             | 45 +++++++++++++------
 1 file changed, 32 insertions(+), 13 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 189f9e6b..3cccc3a4 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -5,6 +5,8 @@
 (if (ros::resolve-ros-path "package://hironx_ros_bridge")
   (ros::load-ros-manifest "hironx_ros_bridge"))
 
+(defconstant +hironx-hand-servo-num+ 4)
+
 (defclass hironxjsk-interface
   :super rtm-ros-robot-interface
   :slots ())
@@ -252,8 +254,16 @@
 ;; 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
-   (hand &optional av (tm 1000))
+  (:hand-angle-vector (hand &optional av (tm 1000))
+   ;; check type
+   (when av
+     (case hand
+           (:hands
+            (if (= (length av) +hironx-hand-servo-num+) (setq av (concatenate float-vector av av)))
+            (assert (= (length av) (* 2 +hironx-hand-servo-num+))))
+           ((:rhand :lhand)
+            (assert (= (length av) +hironx-hand-servo-num+)))))
+
    ;; simulation mode
    (when (send self :simulation-modep)
      (flet ((get-joint-list (hand)
@@ -264,8 +274,6 @@
                            (:hands (append (get-joint-list :rhand) (get-joint-list :lhand)))
                            ((:rhand :lhand) (get-joint-list hand))
                            (t (error ";; No such hand: ~A~%." hand)))))
-         (if (and (eql hand :hands) (= (length av) 4))
-             (setq av (concatenate float-vector av av)))
          (return-from :hand-angle-vector
            (if av
              ;; setjointangles
@@ -280,7 +288,6 @@
      (let ((av-rad-list (map cons #'deg2rad av)))
        (case hand
              (:hands
-              (when (= (length av) 4) (setq av-rad-list (concatenate float-vector av-rad-list av-rad-list)))
               (send self :call-operation-return :servocontrollerservice_setjointangles
                            :jvs av-rad-list :tm (/ tm 1000.0)))
              ((:rhand :lhand)
@@ -321,22 +328,33 @@
                      (:lhand (list 6 7 8 9))
                      (t (error ";; No such hand: ~A~%." hand)))))
       (cond
-       ((numberp effort)
+       ((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))
-       ;; setmaxtorque with different effort values
-       (effort
+       ((or (consp effort) (vectorp effort))
+        ;; check length
+        (case hand
+              (:hands
+               (if (= (length effort) +hironx-hand-servo-num+)
+                   (setq effort (concatenate float-vector effort effort)))
+               (assert (= (length effort) (* 2 +hironx-hand-servo-num+))))
+              ((:rhand :lhand)
+               (assert (= (length effort) +hironx-hand-servo-num+))))
+        ;; 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))
-       ;; getmaxtorque
        (t
-        (mapcar
-         #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage))
-         ids)))))
+        ;; unsupported type
+        (error "number or sequence expected")))))
   (:hand-width2angles (width)
     (let ((safetymargin 3) (w0 19) (l1 41.9))
       (unless (<= (- safetymargin) width %(2 * (w0 + l1 - safetymargin)))
@@ -345,7 +363,8 @@
       (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
         (float-vector a (- a) (- a) a))))
   (:hand-angles2width (vec)
-    (assert (= (length vec) 4) "Expecting vector of length 4~%")
+    (assert (= (length vec) +hironx-hand-servo-num+)
+            (format nil "Expecting vector of length ~a~%" +hironx-hand-servo-num+))
     (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)

From 59d365de5a6b95cd4ad7432067d788a62f471918 Mon Sep 17 00:00:00 2001
From: Guilherme Affonso <guilherme.c.affonso@gmail.com>
Date: Thu, 4 Apr 2019 21:52:29 +0900
Subject: [PATCH 23/23] Add printing information to typecheck

---
 .../euslisp/hironxjsk-interface.l             | 27 ++++++++++---------
 1 file changed, 15 insertions(+), 12 deletions(-)

diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
index 3cccc3a4..607d05ed 100644
--- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
+++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l
@@ -5,11 +5,9 @@
 (if (ros::resolve-ros-path "package://hironx_ros_bridge")
   (ros::load-ros-manifest "hironx_ros_bridge"))
 
-(defconstant +hironx-hand-servo-num+ 4)
-
 (defclass hironxjsk-interface
   :super rtm-ros-robot-interface
-  :slots ())
+  :slots (hand-servo-num))
 
 ;; Initialize
 (defmethod hironxjsk-interface
@@ -31,7 +29,9 @@
               (format nil "~A/models/~A-force-moment-offset.l"
                       (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials")
                       (send robot :name))
-              :set-offset t))))
+              :set-offset t))
+      ;; number of servo motors in one hand
+      (setq hand-servo-num 4)))
   (:define-all-ROSBridge-srv-methods
     (&key (debug-view nil) (ros-pkg-name "hrpsys_ros_bridge"))
     ;; First, define ROSBridge method for old impedance controller
@@ -254,15 +254,19 @@
 ;; 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))
+   (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))
    ;; check type
    (when av
      (case hand
            (:hands
-            (if (= (length av) +hironx-hand-servo-num+) (setq av (concatenate float-vector av av)))
-            (assert (= (length av) (* 2 +hironx-hand-servo-num+))))
+            (if (= (length av) hand-servo-num) (setq av (concatenate float-vector av av)))
+            (send self :check-hand-vector-length av 2))
            ((:rhand :lhand)
-            (assert (= (length av) +hironx-hand-servo-num+)))))
+            (send self :check-hand-vector-length av))))
 
    ;; simulation mode
    (when (send self :simulation-modep)
@@ -342,11 +346,11 @@
         ;; check length
         (case hand
               (:hands
-               (if (= (length effort) +hironx-hand-servo-num+)
+               (if (= (length effort) hand-servo-num)
                    (setq effort (concatenate float-vector effort effort)))
-               (assert (= (length effort) (* 2 +hironx-hand-servo-num+))))
+               (send self :check-hand-vector-length effort 2))
               ((:rhand :lhand)
-               (assert (= (length effort) +hironx-hand-servo-num+))))
+               (send self :check-hand-vector-length effort)))
         ;; setmaxtorque with different effort values
         (map cons
          #'(lambda (id val)
@@ -363,8 +367,7 @@
       (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1)))))
         (float-vector a (- a) (- a) a))))
   (:hand-angles2width (vec)
-    (assert (= (length vec) +hironx-hand-servo-num+)
-            (format nil "Expecting vector of length ~a~%" +hironx-hand-servo-num+))
+    (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)