diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 45a71b9ce..c70381ec1 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -260,13 +260,16 @@ (:pick-object-with-movable-region (arm movable-region &key (n-trial 1) (n-trial-same-pos 1) (do-stop-grasp nil) (grasp-style :suction)) + (send *ri* :calib-proximity-threshold arm) (let (graspingp avs object-index obj-pos obj-cube pinch-yaw) ;; TODO: object-index is set randomly (setq object-index (random (length (gethash arm object-boxes-)))) (setq obj-pos (send self :get-object-position arm movable-region :object-index object-index)) (setq obj-cube (send self :bbox->cube (elt (gethash arm object-boxes-) object-index))) - (if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) (setq pinch-yaw pi/2) (setq pinch-yaw 0)) + (if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) + (setq pinch-yaw (+ pi/2 (caar (send (send obj-cube :worldcoords) :rpy-angle)))) + (setq pinch-yaw (caar (send (send obj-cube :worldcoords) :rpy-angle)))) (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm) (send *ri* :gripper-servo-on arm) ;; Setup arm for picking diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index a4bc7a361..d311d94da 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -15,11 +15,14 @@ :super baxter-interface :slots (rarm-pressure-threshold- larm-pressure-threshold- + proximity-threshold- right-hand-action- pressure- finger-flex- finger-load- - prismatic-load-)) + prismatic-load- + proximities- + proximity-init-values-)) (defmethod jsk_arc2017_baxter::baxter-interface (:init @@ -72,6 +75,8 @@ (setq finger-flex- (make-hash-table)) (setq prismatic-load- (make-hash-table)) (setq prismatic-vel- (make-hash-table)) + (setq proximities- (make-hash-table)) + (setq proximity-init-values- (make-hash-table)) (dolist (arm (list :rarm :larm)) (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm)) std_msgs::Float64 @@ -87,7 +92,13 @@ dynamixel_msgs::JointState #'send self :set-prismatic-state arm :groupname groupname) + (ros::subscribe (format nil "/gripper_front/limb/~a/proximity_array" + (arm2str arm)) + force_proximity_ros::ProximityArray + #'send self :set-proximities arm + :groupname groupname) (sethash arm finger-flex- (make-hash-table)) + (sethash arm proximity-init-values- (make-hash-table)) (dolist (side (list :right :left)) (ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state" (arm2str arm) (symbol2str side)) @@ -153,6 +164,9 @@ (:set-finger-flex (arm side msg) (sethash side (gethash arm finger-flex-) (send msg :data))) + (:set-proximities + (arm msg) + (sethash arm proximities- (send msg :proximities))) ;; Hand interface ;; based on naoqi-interface and fetch-interface @@ -184,6 +198,10 @@ res) nil) nil)) + (:cancel-move-hand() + (send right-hand-action- :cancel-goal)) + (:hand-interpolatingp () + (eq (send right-hand-action- :get-state) ros::*simple-goal-state-active*)) (:get-finger-flex (arm side) (send self :spin-once) (gethash side (gethash arm finger-flex-))) @@ -196,6 +214,22 @@ (:get-prismatic-vel (arm) (send self :spin-once) (gethash arm prismatic-vel-)) + (:get-proximity (arm side &key (raw nil)) + (send self :spin-once) + (let ((rl (gethash :left (gethash arm proximity-init-values-))) + (rr (gethash :right (gethash arm proximity-init-values-))) + ;;(rm (gethash :middle (gethash arm proximity-init-values-))) + (proximities (gethash arm proximities-))) + (if (null raw) + (cond + ;; for sparkfun proximity sensor + ((eq side :left) + (/ (- (send (elt proximities 0) :average) rl) (expt (/ rl 2500.0) 1.5))) + ((eq side :right) + (/ (- (send (elt proximities 1) :average) rr) (expt (/ rr 2500.0) 1.5)))) + (cond + ((eq side :left) (send (elt proximities 0) :average)) + ((eq side :right) (send (elt proximities 1) :average)))))) (:get-real-finger-av (arm) (send self :update-robot-state :wait-until-update t) (float-vector @@ -225,9 +259,55 @@ ((eq type :pinch) (send self :update-robot-state :wait-until-update t) (let ((finger-av (send self :get-real-finger-av l/r)) - (prev-av (send robot :angle-vector)) avs) + prev-av av avs hand-interpolatingp) + ;; rotate arm using proximity sensor (setf (aref finger-av 1) 180) - (send self :move-hand l/r finger-av 1000) + (send self :move-hand l/r finger-av 2000 :wait nil) + (dotimes (x 100) + (if (send self :interpolatingp) (return)) + (unix::usleep 1000)) + (while (and + (< (max (send self :get-proximity l/r :right) + (send self :get-proximity l/r :left)) + proximity-threshold-) + (setq hand-interpolatingp (send self :hand-interpolatingp))) + (unix::usleep 1000)) + (when hand-interpolatingp + (send self :cancel-move-hand) + (send self :update-robot-state :wait-until-update t) + (setq prev-av (send robot :angle-vector)) + (if (< (send self :get-proximity l/r :right) (send self :get-proximity l/r :left)) + (progn + (send robot l/r :wrist-r :joint-angle 45 :relative t) + (setq av (send robot :angle-vector)) + (send robot :angle-vector prev-av) + (send self :angle-vector-raw av 2000) + (send self :move-hand l/r finger-av 4000 :wait nil) + (dotimes (x 100) + (if (send self :interpolatingp) (return)) + (unix::usleep 1000)) + (while (and (< (send self :get-proximity l/r :right) + (send self :get-proximity l/r :left)) + (send self :interpolatingp)) + (unix::usleep 1000))) + (progn + (send robot l/r :wrist-r :joint-angle -45 :relative t) + (setq av (send robot :angle-vector)) + (send robot :angle-vector prev-av) + (send self :angle-vector-raw av 2000) + (send self :move-hand l/r finger-av 4000 :wait nil) + (dotimes (x 100) + (if (send self :interpolatingp) (return)) + (unix::usleep 1000)) + (while (and (> (send self :get-proximity l/r :right) + (send self :get-proximity l/r :left)) + (send self :interpolatingp)) + (unix::uleep 1000))) + (send self :cancel-move-hand) + (send self :cancel-angle-vector) + (send self :update-robot-state :wait-until-update t) + (send self :move-hand l/r finger-av 1000)) + (setq prev-av (send robot :angle-vector)) (when (> (aref finger-av 0) 45) ;; if cylindrical and spherical grasp, move other gripper joints (send robot l/r :gripper-p :joint-angle -90) @@ -236,7 +316,7 @@ (pushback (send robot :angle-vector) avs) (send robot :angle-vector prev-av) (send self :angle-vector-sequence-raw avs) - (send self :wait-interpolation))))))) + (send self :wait-interpolation)))))))) (:stop-grasp (&optional (arm :arms) (type :suction)) (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm))) @@ -392,7 +472,17 @@ (setq rarm-pressure-threshold- (- min-pressure 15)))))) (send self :stop-grasp arm) (ros::ros-info "[:calib-pressure-threshold] Threshold r: ~a l: ~a" - rarm-pressure-threshold- larm-pressure-threshold-))) + rarm-pressure-threshold- larm-pressure-threshold-)) + (:calib-proximity-threshold + (&optional (arm :arms)) + (send self :spin-once) + (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm))) + (let ((hash-table (make-hash-table))) + (sethash :left hash-table (send self :get-proximity l/r :left :raw t)) + (sethash :right hash-table (send self :get-proximity l/r :right :raw t)) + (sethash :middle hash-table (send self :get-proximity l/r :middle :raw t)) + (sethash l/r proximity-init-values- hash-table)) + (setq proximity-threshold- 100)))) ;; TODO decide proper threshold 100 -> ??? (defclass jsk_arc2017_baxter::baxter-moveit-environment diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter.l b/jsk_arc2017_baxter/euslisp/lib/baxter.l index 86fa0bd00..7d5c34074 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter.l @@ -109,15 +109,16 @@ (case (car args) ((:angle-vector) (let ((av (cadr args)) (joints (gethash arm hand-joints-))) - (when av - (dotimes (i (length av)) - (send self (elt joints i) :joint-angle (elt av i)) - (send self (elt joints (+ i (length av))) :joint-angle (elt av i)))) + (if (and (null (eq (length av) 0)) (null (eq (length av) 2))) + (progn (ros::ros-error "length of angle-vector must be 0 or 2.~%") (exit))) + (dotimes (i (length av)) + (send self (elt joints i) :joint-angle (elt av i)) + (send self (elt joints (+ i (length av))) :joint-angle (elt av i))) (mapcar #'(lambda (j) (send self j :joint-angle)) (subseq joints 0 (/ (length joints) 2))) )) - (t (error ":hand first arg is invalid. args: ~A~%" args)) + (t (ros::ros-error ":hand first arg is invalid. args: ~A~%" args) (exit)) )) (:hand-grasp-pre-pose (arm style) @@ -125,7 +126,7 @@ (:opposed (send self :hand arm :angle-vector #f(0 0))) (:spherical (send self :hand arm :angle-vector #f(30 0))) (:cylindrical (send self :hand arm :angle-vector #f(90 0))) - (t (error ":hand-grasp-pre-pose no such style ~A~%" style)) + (t (ros::ros-error ":hand-grasp-pre-pose no such style ~A~%" style) (exit)) )) (:hand-grasp-pose (arm style &key (angle 180)) @@ -133,7 +134,7 @@ (:opposed (send self :hand arm :angle-vector (float-vector 0 angle))) (:spherical (send self :hand arm :angle-vector (float-vector 30 angle))) (:cylindrical (send self :hand arm :angle-vector (float-vector 90 angle))) - (t (error ":hand-grasp-pose no such style ~A~%" style)) + (t (ros::ros-error ":hand-grasp-pose no such style ~A~%" style) (exit)) )) (:avoid-shelf-pose (arm bin)