diff --git a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l index 0239b45b..607d05ed 100644 --- a/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l @@ -7,7 +7,7 @@ (defclass hironxjsk-interface :super rtm-ros-robot-interface - :slots ()) + :slots (hand-servo-num)) ;; Initialize (defmethod hironxjsk-interface @@ -24,11 +24,14 @@ (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)) + ;; 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 @@ -251,13 +254,45 @@ ;; 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 - (&optional (hand :hands) av (tm 1000)) + (: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) hand-servo-num) (setq av (concatenate float-vector av av))) + (send self :check-hand-vector-length av 2)) + ((:rhand :lhand) + (send self :check-hand-vector-length av)))) + + ;; simulation mode + (when (send self :simulation-modep) + (flet ((get-joint-list (hand) + (let (acc) + (dotimes (i 4) (push (read-from-string (format nil "~a_joint~a" hand i)) acc)) + (nreverse acc)))) + (let ((joint-list (case hand + (:hands (append (get-joint-list :rhand) (get-joint-list :lhand))) + ((:rhand :lhand) (get-joint-list hand)) + (t (error ";; No such hand: ~A~%." hand))))) + (return-from :hand-angle-vector + (if av + ;; setjointangles + (map nil #'(lambda (joint angle) (send robot joint :joint-angle angle)) + joint-list av) + ;; getjointangles + (map float-vector #'(lambda (joint) (send robot joint :joint-angle)) + joint-list)))))) + ;; real robot (if av ;; setjointangles (let ((av-rad-list (map cons #'deg2rad av))) (case hand - (:hands (send self :call-operation-return :servocontrollerservice_setjointangles + (:hands + (send self :call-operation-return :servocontrollerservice_setjointangles :jvs av-rad-list :tm (/ tm 1000.0))) ((:rhand :lhand) (send self :call-operation-return :servocontrollerservice_setjointanglesofgroup @@ -283,60 +318,88 @@ (* 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" + "effort is percentage or sequence of percentages" + (if (send self :simulation-modep) (return-from :hand-effort nil)) (let ((ids (case hand (:hands (list 2 3 4 5 6 7 8 9)) (:rhand (list 2 3 4 5)) (:lhand (list 6 7 8 9)) (t (error ";; No such hand: ~A~%." hand))))) - (if effort - ;; setmaxtorque - (mapcar - #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort)) - ids) + (cond + ((null effort) ;; getmaxtorque (mapcar #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage)) - ids)))) + ids)) + ((and (numberp effort) (plusp effort)) + ;; setmaxtorque with same effort value + (mapcar + #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort)) + ids)) + ((or (consp effort) (vectorp effort)) + ;; check length + (case hand + (:hands + (if (= (length effort) hand-servo-num) + (setq effort (concatenate float-vector effort effort))) + (send self :check-hand-vector-length effort 2)) + ((:rhand :lhand) + (send self :check-hand-vector-length effort))) + ;; setmaxtorque with different effort values + (map cons + #'(lambda (id val) + (if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val))) + ids effort)) + (t + ;; unsupported type + (error "number or sequence expected"))))) (:hand-width2angles (width) - (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 (<= (- safetymargin) 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)) - (: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))))) + (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1))))) + (float-vector a (- a) (- a) a)))) + (:hand-angles2width (vec) + (send self :check-hand-vector-length vec) + (let ((safetymargin 3) (w0 19) (l1 41.9) (l2 20)) + (flet ((get-width (r1 r2) %( w0 + l1 * cos(pi/2 - r1) + l2 * cos(pi/2 - r1 - r2) - safetymargin))) + (multiple-value-bind (a1 a2 b1 b2) (map cons #'deg2rad vec) + (+ (get-width a1 a2) + (get-width (- b1) (- b2))))))) + (:hand-width (hand &optional width &key (time 1000) effort) + (if width + ;; set hand width + (progn + (when effort (send self :hand-effort hand effort)) + (send self :hand-angle-vector hand (send self :hand-width2angles width) time)) + ;; get hand width + (send self :hand-angles2width (send self :hand-angle-vector hand)))) + (:start-grasp (&optional (arm :arms) &key (time 1000) effort) + (case arm + (:arms (setq arm :hands)) + (:rarm (setq arm :rhand)) + (:larm (setq arm :lhand))) + (send self :hand-width arm 0 :time time :effort effort)) + (:stop-grasp (&optional (arm :arms) &key (time 1000) effort) + (case arm + (:arms (setq arm :hands)) + (:rarm (setq arm :rhand)) + (:larm (setq arm :lhand))) + (send self :hand-width arm 100 :time time :effort effort))) (defun hironxjsk-init (&rest args) (if (not (boundp '*ri*)) (setq *ri* (instance* hironxjsk-interface :init args))) (if (not (boundp '*hironxjsk*)) - (setq *hironxjsk* (instance hironxjsk-robot :init)))) + (setq *hironxjsk* (instance hironxjsk-robot :init))) + ;; read initial robot state + (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector)) + ;; return robot instance + *hironxjsk*)