From 1f86a1cce7c3d7643ac466b30740b64d06e679d2 Mon Sep 17 00:00:00 2001 From: Guilherme Date: Mon, 3 Dec 2018 18:36:16 +0900 Subject: [PATCH 01/12] 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 bc3363a19424238067d6856460433188ffa456e8 Mon Sep 17 00:00:00 2001 From: Guilherme Date: Mon, 3 Dec 2018 19:28:54 +0900 Subject: [PATCH 02/12] 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 a0ca0e8f28fc2f1647628496313b5bb2108fecd3 Mon Sep 17 00:00:00 2001 From: Guilherme Date: Tue, 4 Dec 2018 14:12:32 +0900 Subject: [PATCH 03/12] 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 0b9949b58754a5bc3762a735d5878686bd49148f Mon Sep 17 00:00:00 2001 From: Guilherme Date: Tue, 4 Dec 2018 14:47:25 +0900 Subject: [PATCH 04/12] 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 32ddc9bc1f48a2b22fd5dade29fae291374e6eb8 Mon Sep 17 00:00:00 2001 From: Guilherme Date: Tue, 4 Dec 2018 15:04:16 +0900 Subject: [PATCH 05/12] 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 9f9067ff9c7a9bd3511c5bc29d807e627c7113ad Mon Sep 17 00:00:00 2001 From: Guilherme Date: Tue, 4 Dec 2018 19:32:56 +0900 Subject: [PATCH 06/12] 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 eccd72416dfe036f626eb6837d8b1503f78b005f Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 5 Dec 2018 17:04:54 +0900 Subject: [PATCH 07/12] 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 d98733692768c2971d8c97d9fec0df7279d687fc Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Dec 2018 21:29:39 +0900 Subject: [PATCH 08/12] 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 9a4d49ec60bb34ff7977087aa042eb47e3c4d16b Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 20 Dec 2018 22:40:43 +0900 Subject: [PATCH 09/12] 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 8a9995567b101cdc2cce83f91c7687143b4b8eec Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Fri, 21 Dec 2018 10:09:02 +0900 Subject: [PATCH 10/12] 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 1e700842f79624ab399b38bb20523673c273c4a3 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 3 Apr 2019 18:25:32 +0900 Subject: [PATCH 11/12] 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 4ddedec446628ce74dc146641e01028d1f5f0aad Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Thu, 4 Apr 2019 21:52:29 +0900 Subject: [PATCH 12/12] 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)