diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 0d5bc5f9..19529eaf 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/hrpsys_ros_bridge_tutorials/CMakeLists.txt @@ -277,6 +277,28 @@ if(EXISTS ${hrp2_models_MODEL_DIR}/HRP3HAND_R/HRP3HAND_Rmain.wrl) HRP3HAND_R) endif() +# HIRONXJSK +compile_openhrp_model_for_closed_robots(HIRONXJSK HIRONXJSK HIRONXJSK + --conf-dt-option "0.005" + --conf-file-option "# This conf should be backup of /opt/jsk/etc/HIRONX/hrprtc/Robot.conf inside HIRONXJSK" + --conf-file-option "# model should be /opt/jsk/etc/HIRONX/model/main.wrl inside HIRONXJSK" + --conf-file-option "pdgains.file_name: /opt/jsk/etc/HIRONX/hrprtc/PDgains.sav" + --conf-file-option "naming.formats: %n.rtc" + --conf-file-option "# RTC disable/enable settings which will be loaded via RobotHardware" + --conf-file-option "CollisionDetector.enable: YES" + --conf-file-option "# for HIRO hand servo" + --conf-file-option "servo.id: 2,3,4,5, 6,7,8,9" + --conf-file-option "servo.offset: -0.80,0.0,-0.85,0.0, -0.85,0.0,-0.89,0.0" + --conf-file-option "servo.devname: /dev/serusb1" + --conf-file-option "servo.dir 1,1,-1,-1,1,1,-1,-1" + --conf-file-option "# for RemoveForceSensorLinkOffset" + --conf-file-option "forcemoment_offset_params: /opt/jsk/etc/HIRONX/hrprtc/force_sensor_calib_20191127" + --conf-file-option "# for ImpedanceController" + --conf-file-option "end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708, larm,LARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708," + --conf-file-option "# for CollisionDetector" + --conf-file-option "collision_pair: HEAD_JOINT0:RARM_JOINT2 HEAD_JOINT0:RARM_JOINT3 HEAD_JOINT0:RARM_JOINT4 HEAD_JOINT0:RARM_JOINT5 HEAD_JOINT0:LARM_JOINT2 HEAD_JOINT0:LARM_JOINT3 HEAD_JOINT0:LARM_JOINT4 HEAD_JOINT0:LARM_JOINT5 HEAD_JOINT1:RARM_JOINT2 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:LARM_JOINT2 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 WAIST:RARM_JOINT3 WAIST:RARM_JOINT4 WAIST:RARM_JOINT5 WAIST:LARM_JOINT3 WAIST:LARM_JOINT4 WAIST:LARM_JOINT5 CHEST_JOINT0:RARM_JOINT2 CHEST_JOINT0:RARM_JOINT3 CHEST_JOINT0:RARM_JOINT4 CHEST_JOINT0:RARM_JOINT5 CHEST_JOINT0:LARM_JOINT2 CHEST_JOINT0:LARM_JOINT3 CHEST_JOINT0:LARM_JOINT4 CHEST_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT1 RARM_JOINT0:LARM_JOINT2 RARM_JOINT0:LARM_JOINT3 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT1:LARM_JOINT0 RARM_JOINT1:LARM_JOINT1 RARM_JOINT1:LARM_JOINT2 RARM_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT2:LARM_JOINT0 RARM_JOINT2:LARM_JOINT1 RARM_JOINT2:LARM_JOINT2 RARM_JOINT2:LARM_JOINT3 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT3:LARM_JOINT0 RARM_JOINT3:LARM_JOINT1 RARM_JOINT3:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5" + ) + # URATALEG compile_rbrain_model_for_closed_robots(URATALEG URATALEG URATALEG --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav" 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..58e50ba4 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-interface.l @@ -0,0 +1,189 @@ +(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l") +(require :hironxjsk "package://hrpsys_ros_bridge_tutorials/models/hironxjsk.l") +(when (probe-file (ros::resolve-ros-path "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l")) + (require :hironxjsk-utils "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l")) + +(defclass hironxjsk-interface + :super rtm-ros-robot-interface + :slots (hand-servo-num)) + +;; Initialize +(defmethod hironxjsk-interface + ;; Based on https://github.com/start-jsk/rtmros_tutorials/blob/9132c58702b3b193e14271b4c231ad0080187850/hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l + (:init (&rest args) + (prog1 + ;; Hironx has two types of joint_states on one topic: whole body and hand, + ;; so queue size of joint_states should be two. + ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L120 + (send-super* :init :joint-states-queue-size 2 :robot hironxjsk-robot args) + ;; add controller + (dolist (limb '(:rarm :larm :head :torso)) + (send self :def-limb-controller-method limb) + (send self :add-controller (read-from-string (format nil "~A-controller" limb)) + :joint-enable-check t :create-actions t)) + ;; number of servo motors in one hand + (setq hand-servo-num 4))) + (:call-operation-return (method &rest args) + ;; Call method until it returns true + ;; Used to ensure operation on the hand service calls, that sometimes fail + (do ((res (send* self method args) + (send* self method args))) + ((send res :operation_return) res)))) + +;; ServoControllerService for hand +;; Based on https://github.com/start-jsk/rtmros_hironx/blob/2.1.0/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +;; and https://github.com/start-jsk/rtmros_tutorials/blob/0.1.6/hrpsys_ros_bridge_tutorials/euslisp/hrp2-common-interface.l +(defmethod hironxjsk-interface + (:check-hand-vector-length (vec &optional (hand-num 1)) + ;; Ensure that `vec' is a 4 element vector for single hand or 8 element for double hand + (let ((len (* hand-num hand-servo-num))) + (assert (= (length vec) len) + "[ERROR] Expecting vector of length ~a~%" len))) + (:hand-angle-vector (hand &optional av (tm 1000)) + (when av + ;; check type + (case hand + (:hands + (if (= (length av) hand-servo-num) (setq av (concatenate float-vector av av))) + (send self :check-hand-vector-length av 2)) + ((:rhand :lhand) + (send self :check-hand-vector-length av)))) + + ;; simulation mode + (when (send self :simulation-modep) + (flet ((get-joint-list (hand) + (let (acc) + (dotimes (i 4) (push (read-from-string (format nil "~a_joint~a" hand i)) acc)) + (nreverse acc)))) + (let ((joint-list (case hand + (:hands (append (get-joint-list :rhand) (get-joint-list :lhand))) + ((:rhand :lhand) (get-joint-list hand)) + (t (error ";; No such hand: ~A~%." hand))))) + (return-from :hand-angle-vector + (if av + ;; setjointangles + (map nil #'(lambda (joint angle) (send robot joint :joint-angle angle)) + joint-list av) + ;; getjointangles + (map float-vector #'(lambda (joint) (send robot joint :joint-angle)) + joint-list)))))) + ;; real robot + (if av + ;; setjointangles + (let ((av-rad-list (map cons #'deg2rad av))) + (case hand + (:hands + (send self :call-operation-return :servocontrollerservice_setjointangles + :jvs av-rad-list :tm (/ tm 1000.0))) + ((:rhand :lhand) + (send self :call-operation-return :servocontrollerservice_setjointanglesofgroup + :gname (string-downcase hand) :jvs av-rad-list :tm (/ tm 1000.0))) + (t (error ";; No such hand: ~A~%." hand)))) + ;; getjointangles + (let ((ids (case hand + (:hands (list 2 3 4 5 6 7 8 9)) + (:rhand (list 2 3 4 5)) + (:lhand (list 6 7 8 9)) + (t (error ";; No such hand: ~A~%." hand)))) + (dirs (case hand + (:hands #f(1 1 -1 -1 1 1 -1 -1)) + ((:lhand :rhand) #f(1 1 -1 -1))))) + ;; servocontrollerservice_getjointangles do not consider servo offset and direction + ;; servocontrollerservice_getjointangle do not consider servo direction + ;; defined in /opt/jsk/etc/HIRONX/hrprtc/Robot.conf + ;; servo.id: 2,3,4,5, 6,7,8,9 + ;; servo.offset: -0.78,0.0,-0.82,0.0, -0.85,0.0,-0.82,0.0 + ;; servo.dir 1,1,-1,-1,1,1,-1,-1 + (map float-vector + #'(lambda (id dir) + (* dir (send (send self :call-operation-return :servocontrollerservice_getjointangle :id id) :jv))) + ids dirs)))) + (:hand-servo-on () + (unless (send self :simulation-modep) + (send self :call-operation-return :servocontrollerservice_servoon))) + (:hand-servo-off () + (unless (send self :simulation-modep) + (send self :call-operation-return :servocontrollerservice_servooff))) + (:hand-effort (&optional (hand :hands) effort) + ;; effort is percentage or sequence of percentages + (if (send self :simulation-modep) (return-from :hand-effort nil)) + (let ((ids (case hand + (:hands (list 2 3 4 5 6 7 8 9)) + (:rhand (list 2 3 4 5)) + (:lhand (list 6 7 8 9)) + (t (error ";; No such hand: ~A~%." hand))))) + (cond + ((null effort) + ;; getmaxtorque + (mapcar + #'(lambda (id) (send (send self :call-operation-return :servocontrollerservice_getmaxtorque :id id) :percentage)) + ids)) + ((and (numberp effort) (plusp effort)) + ;; setmaxtorque with same effort value + (mapcar + #'(lambda (id) (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage effort)) + ids)) + ((or (consp effort) (vectorp effort)) + ;; check length + (case hand + (:hands + (if (= (length effort) hand-servo-num) + (setq effort (concatenate float-vector effort effort))) + (send self :check-hand-vector-length effort 2)) + ((:rhand :lhand) + (send self :check-hand-vector-length effort))) + ;; setmaxtorque with different effort values + (map cons + #'(lambda (id val) + (if val (send self :call-operation-return :servocontrollerservice_setmaxtorque :id id :percentage val))) + ids effort)) + (t + ;; unsupported type + (error "number or sequence expected"))))) + (:hand-width2angles (width) + ;; Calculates the hand angles to achieve a certain parallel aperture + (let ((safetymargin 3) (w0 19) (l1 41.9)) + (unless (<= (- safetymargin) width %(2 * (w0 + l1 - safetymargin))) + (warn ";; width value ~a is off margins~%" width) + (return-from :hand-width2angles nil)) + (let ((a (rad2deg %(pi/2 - acos((width / 2.0 + safetymargin - w0) / l1))))) + (float-vector a (- a) (- a) a)))) + (:hand-angles2width (vec) + ;; Calculates the hand aperture given a certain angle vector + (send self :check-hand-vector-length vec) + (let ((safetymargin 3) (w0 19) (l1 41.9) (l2 20)) + (flet ((get-width (r1 r2) %( w0 + l1 * cos(pi/2 - r1) + l2 * cos(pi/2 - r1 - r2) - safetymargin))) + (multiple-value-bind (a1 a2 b1 b2) (map cons #'deg2rad vec) + (+ (get-width a1 a2) + (get-width (- b1) (- b2))))))) + (:hand-width (hand &optional width &key (time 1000) effort) + ;; Get/Set the hand width + (if width + ;; set hand width + (progn + (when effort (send self :hand-effort hand effort)) + (send self :hand-angle-vector hand (send self :hand-width2angles width) time)) + ;; get hand width + (send self :hand-angles2width (send self :hand-angle-vector hand)))) + (:start-grasp (&optional (arm :arms) &key (time 1000) effort) + (case arm + (:arms (setq arm :hands)) + (:rarm (setq arm :rhand)) + (:larm (setq arm :lhand))) + (send self :hand-width arm 0 :time time :effort effort)) + (:stop-grasp (&optional (arm :arms) &key (time 1000) effort) + (case arm + (:arms (setq arm :hands)) + (:rarm (setq arm :rhand)) + (:larm (setq arm :lhand))) + (send self :hand-width arm 100 :time time :effort effort))) + +(defun hironxjsk-init (&rest args) + (if (not (boundp '*ri*)) + (setq *ri* (instance* hironxjsk-interface :init args))) + (if (not (boundp '*hironxjsk*)) + (setq *hironxjsk* (instance hironxjsk-robot :init))) + ;; read initial robot state + (send *hironxjsk* :angle-vector (send *ri* :state :potentio-vector)) + ;; return robot instance + *hironxjsk*) 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]