From 87a90ae5c5311e8a743620f9c8df394927b98679 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 9 Jul 2017 21:52:04 +0900 Subject: [PATCH] Revert #2235 'Grasp using proximity' Because - We cannot use left hand with this change. - Has typo. --- .../euslisp/lib/arc-interface.l | 5 +- .../euslisp/lib/baxter-interface.l | 100 +----------------- jsk_arc2017_baxter/euslisp/lib/baxter.l | 15 ++- 3 files changed, 13 insertions(+), 107 deletions(-) diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l index 2a26ddcc8..4d66f0f4a 100755 --- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l @@ -260,16 +260,13 @@ (: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 (caar (send (send obj-cube :worldcoords) :rpy-angle)))) - (setq pinch-yaw (caar (send (send obj-cube :worldcoords) :rpy-angle)))) + (if (> (x-of-cube obj-cube) (y-of-cube obj-cube)) (setq pinch-yaw pi/2) (setq pinch-yaw 0)) (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 9180600b6..b7066b2b5 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -15,14 +15,11 @@ :super baxter-interface :slots (rarm-pressure-threshold- larm-pressure-threshold- - proximity-threshold- right-hand-action- pressure- finger-flex- finger-load- - prismatic-load- - proximities- - proximity-init-values-)) + prismatic-load-)) (defmethod jsk_arc2017_baxter::baxter-interface (:init @@ -75,8 +72,6 @@ (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 @@ -92,13 +87,7 @@ 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)) @@ -164,9 +153,6 @@ (: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 @@ -198,10 +184,6 @@ 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-))) @@ -214,22 +196,6 @@ (: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 @@ -259,55 +225,9 @@ ((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 av avs hand-interpolatingp) - ;; rotate arm using proximity sensor + (prev-av (send robot :angle-vector)) avs) (setf (aref finger-av 1) 180) - (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)) + (send self :move-hand l/r finger-av 1000) (when (> (aref finger-av 0) 45) ;; if cylindrical and spherical grasp, move other gripper joints (send robot l/r :gripper-p :joint-angle -90) @@ -316,7 +236,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))) @@ -474,17 +394,7 @@ (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-)) - (: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 -> ??? + rarm-pressure-threshold- larm-pressure-threshold-))) (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 7d5c34074..86fa0bd00 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter.l @@ -109,16 +109,15 @@ (case (car args) ((:angle-vector) (let ((av (cadr args)) (joints (gethash arm hand-joints-))) - (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))) + (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)))) (mapcar #'(lambda (j) (send self j :joint-angle)) (subseq joints 0 (/ (length joints) 2))) )) - (t (ros::ros-error ":hand first arg is invalid. args: ~A~%" args) (exit)) + (t (error ":hand first arg is invalid. args: ~A~%" args)) )) (:hand-grasp-pre-pose (arm style) @@ -126,7 +125,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 (ros::ros-error ":hand-grasp-pre-pose no such style ~A~%" style) (exit)) + (t (error ":hand-grasp-pre-pose no such style ~A~%" style)) )) (:hand-grasp-pose (arm style &key (angle 180)) @@ -134,7 +133,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 (ros::ros-error ":hand-grasp-pose no such style ~A~%" style) (exit)) + (t (error ":hand-grasp-pose no such style ~A~%" style)) )) (:avoid-shelf-pose (arm bin)