From 5ae4e29567bd5a81900b7dabf961e3d40183745c Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 17 Jun 2017 18:04:01 +0900
Subject: [PATCH 01/18] Add hand interface methods

---
 .../euslisp/lib/baxter-interface.l               | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index 43a88e0df..336fb76e8 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -139,6 +139,22 @@
           res)
         nil)
       nil))
+  (:get-finger-flex (arm side)
+    (send (one-shot-subscribe
+            (format nil "/gripper_front/limb/~a/flex/~a/state"
+                    (arm2str arm) (symbol2str side))
+            std_msgs::UInt16)
+          :data))
+  (:get-finger-load (arm)
+    (send (one-shot-subscribe
+            (format nil "/gripper_front/limb/~a/dxl/finger_tendon_controller/state"
+                    (arm2str arm))
+            dynamixel_msgs::JointState)
+          :load))
+  (:get-real-finger-roll-angle (arm)
+    (send self :update-robot-state :wait-until-update t)
+    (send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
+          :joint-angle))
 
   (:get-arm-controller (arm)
     (cond ((eq arm :rarm) :rarm-controller)

From e7cf7f9c61e9211e22f76a163804f8e364d3d251 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 17 Jun 2017 18:04:25 +0900
Subject: [PATCH 02/18] Add :wait-interpolation-until

---
 .../euslisp/lib/baxter-interface.l            | 44 +++++++++++++++++++
 1 file changed, 44 insertions(+)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index 336fb76e8..c0a1ff77e 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -228,6 +228,50 @@
         (progn
           (ros::ros-info "[:wait-interpolation-until-grasp] Grasping detected. Cancel angle vector: ~a" arm)
           (send self :cancel-angle-vector)))))
+  (:wait-interpolation-until
+    (arm &rest args)
+    (when (send self :simulation-modep)
+      (return-from :wait-interpolation-until (send self :wait-interpolation)))
+    (unless (or (eq arm :rarm) (eq arm :larm))
+      (error ":wait-interpolation-until set arm for first arg~%"))
+    (let (conds r-init-flex l-init-flex init-load)
+      (setq r-init-flex (send self :get-finger-flex arm :right))
+      (setq l-init-flex (send self :get-finger-flex arm :left))
+      (setq init-load (send self :get-finger-load arm))
+      (ros::ros-info
+        "[:wait-interpolation-until] Init flex:  r: ~a l: ~a" r-init-flex l-init-flex)
+      (ros::ros-info "[:wait-interpolation-until] Init load: ~a" init-load)
+      (when (member :grasp args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for grasp")
+        (pushback #'(lambda () (send self :graspingp arm)) conds))
+      (when (member :finger-flexion args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for finger flexion")
+        (pushback
+          #'(lambda () (or (> (send self :get-finger-flex arm :right) (+ r-init-flex 20))
+                           (> (send self :get-finger-flex arm :left) (+ l-init-flex 20))))
+          conds))
+      (when (member :finger-extension args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for finger extension")
+        (pushback
+          #'(lambda () (or (< (send self :get-finger-flex arm :right) (- r-init-flex 30))
+                           (< (send self :get-finger-flex arm :left) (- l-init-flex 30))))
+          conds))
+      (when (member :finger-loaded args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for finger loaded")
+        (pushback
+          #'(lambda () (> (send self :get-finger-load arm) (+ init-load 0.01)))
+          conds))
+      (when (member :finger-unloaded args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for finger unloaded")
+        (pushback
+          #'(lambda () (< (send self :get-finger-load arm) (- init-load 0.01)))
+          conds))
+      ;; wait for :interpolatingp
+      (unix::usleep 1000)
+      (while (send self :interpolatingp)
+        (when (reduce #'(lambda (x y) (or x y)) (mapcar #'funcall conds))
+          (ros::ros-info "[:wait-interpolation-until-grasp] Cancel angle vector: ~a" arm)
+          (send self :cancel-angle-vector)))))
   (:calib-pressure-threshold
     (&optional (arm :arms))
     (send self :start-grasp arm)

From ab9eade666b9a435c0f072b2f20d2958bdaae25d Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 15:45:54 +0900
Subject: [PATCH 03/18] Enable to set palm endpoint as move-target in IK

---
 jsk_arc2017_baxter/euslisp/lib/baxter.l | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter.l b/jsk_arc2017_baxter/euslisp/lib/baxter.l
index 82012fa63..1a278b0a5 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter.l
@@ -52,6 +52,9 @@
                    :right_gripper_r_finger_yaw_joint
                    :right_gripper_r_finger_roll_joint))
     )
+  (:rarm-palm-endpoint
+    (&rest args)
+    (send* self :right_gripper_palm_endpoint_lk args))
   (:set-ik-prepared-poses
     (poses)
     (if (listp poses)
@@ -66,8 +69,15 @@
         (if (atom move-target)
           (send self :link-list (send move-target :parent))
           (mapcar #'(lambda (mt) (send self :link-list (send mt :parent))) move-target)))
-      (use-gripper nil) (rthre (deg2rad 10))
+      (use-gripper nil) (move-palm-end nil) (rthre (deg2rad 10))
       &allow-other-keys)
+    ;; currently works only if move-target is not list
+    ;; set endpoint of palm as move-target
+    (if move-palm-end
+      (dolist (limb (list :rarm))
+        (when (eq move-target (send self limb :end-coords))
+          (setq move-target (send self limb :palm-endpoint))
+          (setq link-list (send self :link-list (send move-target :parent))))))
     ;; if the last link of link-list is in gripper, remove gripper links
     (if (null use-gripper)
       (cond ((equal (send (car (last link-list)) :name) "right_gripper_pad_with_base")

From b2e7c018a54b42f51ed9e856fd817b50b83cc835 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 17:05:29 +0900
Subject: [PATCH 04/18] Stop using one-shot-subscribe in gripper sensors

---
 .../euslisp/lib/baxter-interface.l            | 61 +++++++++++++------
 1 file changed, 44 insertions(+), 17 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index c0a1ff77e..a94b16973 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -15,7 +15,10 @@
   :super baxter-interface
   :slots (rarm-pressure-threshold-
           larm-pressure-threshold-
-          right-hand-action-))
+          right-hand-action-
+          pressure-
+          finger-flex-
+          finger-load-))
 
 (defmethod jsk_arc2017_baxter::baxter-interface
   (:init
@@ -62,6 +65,27 @@
       (and joint-action-enable (send right-hand-action- :wait-for-server 3))
       (ros::ros-warn "~A is not respond" right-hand-action-)
       (ros::ros-info "*** if you do not have hand, you can ignore this message ***"))
+    ;; gripper sensors
+    (setq pressure- (make-hash-table))
+    (setq finger-load- (make-hash-table))
+    (setq finger-flex- (make-hash-table))
+    (dolist (arm (list :rarm :larm))
+      (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
+                      std_msgs::Float64
+                      #'send self :set-pressure arm
+                      :groupname groupname)
+      (ros::subscribe (format nil "/gripper_front/limb/~a/dxl/finger_tendon_controller/state"
+                              (arm2str arm))
+                      dynamixel_msgs::JointState
+                      #'send self :set-finger-load arm
+                      :groupname groupname)
+      (sethash arm finger-flex- (make-hash-table))
+      (dolist (side (list :right :left))
+        (ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state"
+                                (arm2str arm) (symbol2str side))
+                        std_msgs::UInt16
+                        #'send self :set-finger-flex arm side
+                        :groupname groupname)))
     (if mvit-rb (setq moveit-robot mvit-rb))
     (if mvit-env (send self :set-moveit-environment (send mvit-env :init :robot moveit-robot))))
   ;; Overwrite super class's :rarm-controller
@@ -108,6 +132,15 @@
      (cons :controller-state "/gripper_front/limb/left/state")
      (cons :action-type control_msgs::FollowJointTrajectoryAction)
      (cons :joint-names (list "left_gripper_vacuum_pad_joint")))))
+  (:set-pressure
+    (arm msg)
+    (sethash arm pressure- (send msg :data)))
+  (:set-finger-load
+    (arm msg)
+    (sethash arm finger-load- (send msg :load)))
+  (:set-finger-flex
+    (arm side msg)
+    (sethash side (gethash arm finger-flex-) (send msg :data)))
 
   ;; Hand interface
   ;; based on naoqi-interface and fetch-interface
@@ -140,17 +173,11 @@
         nil)
       nil))
   (:get-finger-flex (arm side)
-    (send (one-shot-subscribe
-            (format nil "/gripper_front/limb/~a/flex/~a/state"
-                    (arm2str arm) (symbol2str side))
-            std_msgs::UInt16)
-          :data))
+    (send self :spin-once)
+    (gethash side (gethash arm finger-flex-)))
   (:get-finger-load (arm)
-    (send (one-shot-subscribe
-            (format nil "/gripper_front/limb/~a/dxl/finger_tendon_controller/state"
-                    (arm2str arm))
-            dynamixel_msgs::JointState)
-          :load))
+    (send self :spin-once)
+    (gethash arm finger-load-))
   (:get-real-finger-roll-angle (arm)
     (send self :update-robot-state :wait-until-update t)
     (send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
@@ -201,8 +228,8 @@
           (send (one-shot-subscribe topic std_msgs::Bool) :data)
           )
         (progn
-          (setq topic (format nil "gripper_front/limb/~a/pressure/state" (arm-to-str arm)))
-          (< (send (one-shot-subscribe topic std_msgs::Float64) :data)
+          (send self :spin-once)
+          (< (gethash arm pressure-)
              (cond
                ((eq arm :rarm) rarm-pressure-threshold-)
                ((eq arm :larm) larm-pressure-threshold-)))))))
@@ -276,11 +303,11 @@
     (&optional (arm :arms))
     (send self :start-grasp arm)
     (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
-      (let ((min-pressure)
-            (topic (format nil "/gripper_front/limb/~a/pressure/state" (arm-to-str l/r))))
+      (let (min-pressure)
         (dotimes (i 7)
-          (let ((pressure (send (one-shot-subscribe topic std_msgs::Float64) :data)))
-            (when (or (null min-pressure) (< pressure min-pressure)) (setq min-pressure pressure)))
+          (send self :spin-once)
+          (when (or (null min-pressure) (< (gethash l/r pressure-) min-pressure))
+            (setq min-pressure (gethash l/r pressure-)))
           (unix::sleep 1))
         (cond ((eq l/r :larm)
                (setq larm-pressure-threshold- (- min-pressure 5)))

From da5ae0ab6e3689049add34d87fbae18fcbf4e051 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 19:13:16 +0900
Subject: [PATCH 05/18] Consider pinching in :graspingp

---
 .../euslisp/lib/baxter-interface.l            | 47 +++++++++++++++----
 1 file changed, 37 insertions(+), 10 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index a94b16973..15c7b9d23 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -18,7 +18,8 @@
           right-hand-action-
           pressure-
           finger-flex-
-          finger-load-))
+          finger-load-
+          prismatic-load-))
 
 (defmethod jsk_arc2017_baxter::baxter-interface
   (:init
@@ -69,6 +70,7 @@
     (setq pressure- (make-hash-table))
     (setq finger-load- (make-hash-table))
     (setq finger-flex- (make-hash-table))
+    (setq prismatic-load- (make-hash-table))
     (dolist (arm (list :rarm :larm))
       (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
                       std_msgs::Float64
@@ -79,6 +81,11 @@
                       dynamixel_msgs::JointState
                       #'send self :set-finger-load arm
                       :groupname groupname)
+      (ros::subscribe (format nil "/gripper_front/limb/~a/dxl/prismatic_joint_controller/state"
+                              (arm2str arm))
+                      dynamixel_msgs::JointState
+                      #'send self :set-prismatic-load arm
+                      :groupname groupname)
       (sethash arm finger-flex- (make-hash-table))
       (dolist (side (list :right :left))
         (ros::subscribe (format nil "/gripper_front/limb/~a/flex/~a/state"
@@ -138,6 +145,9 @@
   (:set-finger-load
     (arm msg)
     (sethash arm finger-load- (send msg :load)))
+  (:set-prismatic-load
+    (arm msg)
+    (sethash arm prismatic-load- (send msg :load)))
   (:set-finger-flex
     (arm side msg)
     (sethash side (gethash arm finger-flex-) (send msg :data)))
@@ -178,10 +188,16 @@
   (:get-finger-load (arm)
     (send self :spin-once)
     (gethash arm finger-load-))
-  (:get-real-finger-roll-angle (arm)
+  (:get-prismatic-load (arm)
+    (send self :spin-once)
+    (gethash arm prismatic-load-))
+  (:get-real-finger-av (arm)
     (send self :update-robot-state :wait-until-update t)
-    (send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
-          :joint-angle))
+    (float-vector
+      (send robot (str2symbol (format nil "~a_gripper_finger_yaw_joint" (arm2str arm)))
+            :joint-angle)
+      (send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
+            :joint-angle)))
 
   (:get-arm-controller (arm)
     (cond ((eq arm :rarm) :rarm-controller)
@@ -220,19 +236,30 @@
         (format nil "/gripper_front/limb/~a/servo/torque" l/r)
         (instance std_msgs::Bool :init :data nil))))
   (:graspingp
-    (arm)
-    (let (topic)
+    (arm &optional type)
+    (let (topic finger-av fingerp)
       (if (ros::get-param "/apc_on_gazebo" nil)
         (progn
           (setq topic (format nil "/robot/~a_vacuum_gripper/grasping" (arm-to-str arm)))
           (send (one-shot-subscribe topic std_msgs::Bool) :data)
           )
         (progn
+          (when (eq arm :rarm)
+            (setq finger-av (send self :get-real-finger-av arm))
+            (setq fingerp
+                  (and (or
+                         (and (< (aref finger-av 0) 45) (< (aref finger-av 1) 80))
+                         (and (>= (aref finger-av 0) 45) (< (aref finger-av 1) 115))
+                         (> (send self :get-prismatic-load arm) 0.25))
+                       (> (send self :get-finger-load arm) 0.55))))
           (send self :spin-once)
-          (< (gethash arm pressure-)
-             (cond
-               ((eq arm :rarm) rarm-pressure-threshold-)
-               ((eq arm :larm) larm-pressure-threshold-)))))))
+          (or (if (not (eq type :suction)) fingerp nil)
+              (if (not (eq type :hand))
+                (< (gethash arm pressure-)
+                   (cond
+                     ((eq arm :rarm) rarm-pressure-threshold-)
+                     ((eq arm :larm) larm-pressure-threshold-)))
+                nil))))))
   (:arm-potentio-vector
     (arm)
     (case arm

From 98cdedbbb158f122ef68498d8216a4d4995696e8 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 19:34:43 +0900
Subject: [PATCH 06/18] Enable :start-grasp and :stop-grasp to move hand

---
 .../euslisp/lib/baxter-interface.l            | 68 ++++++++++++++-----
 1 file changed, 50 insertions(+), 18 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index 15c7b9d23..a46da39bc 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -204,25 +204,57 @@
           ((eq arm :larm) :larm-controller)
           (t nil)))
   (:start-grasp
-    (&optional (arm :arms))
-    (dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))
-      (if (ros::get-param "/apc_on_gazebo" nil)
-        (ros::service-call
-          (format nil "/robot/~a_vacuum_gripper/on" l/r)
-          (instance std_srvs::EmptyRequest :init))
-        (ros::publish
-          (format nil "/vacuum_gripper/limb/~a" l/r)
-          (instance std_msgs::Bool :init :data t)))))
+    (&optional (arm :arms) (type :suction))
+    (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
+      (cond ((eq type :suction)
+             (if (ros::get-param "/apc_on_gazebo" nil)
+               (ros::service-call
+                 (format nil "/robot/~a_vacuum_gripper/on" (arm2str l/r))
+                 (instance std_srvs::EmptyRequest :init))
+               (ros::publish
+                 (format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
+                 (instance std_msgs::Bool :init :data t))))
+            ((eq type :hand)
+             (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)
+               (setf (aref finger-av 1) 180)
+               (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)
+                 (pushback (send robot :angle-vector) avs)
+                 (send robot l/r :gripper-x :joint-angle 0)
+                 (pushback (send robot :angle-vector) avs)
+                 (send robot :angle-vector prev-av)
+                 (send self :angle-vector-sequence-raw avs)
+                 (send self :wait-interpolation)))))))
   (:stop-grasp
-    (&optional (arm :arms))
-    (dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))
-      (if (ros::get-param "/apc_on_gazebo" nil)
-        (ros::service-call
-          (format nil "/robot/~a_vacuum_gripper/off" l/r)
-          (instance std_srvs::EmptyRequest :init))
-        (ros::publish
-          (format nil "/vacuum_gripper/limb/~a" l/r)
-          (instance std_msgs::Bool :init :data nil)))))
+    (&optional (arm :arms) (type :suction))
+    (dolist (l/r (if (eq arm :arms) (list :rarm :larm) (list arm)))
+      (cond ((eq type :suction)
+             (if (ros::get-param "/apc_on_gazebo" nil)
+               (ros::service-call
+                 (format nil "/robot/~a_vacuum_gripper/off" (arm2str l/r))
+                 (instance std_srvs::EmptyRequest :init))
+               (ros::publish
+                 (format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
+                 (instance std_msgs::Bool :init :data nil))))
+            ((eq type :hand)
+             (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)
+               (setf (aref finger-av 1) 0)
+               (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 0)
+                 (pushback (send robot :angle-vector) avs)
+                 (send robot l/r :gripper-x :joint-angle 0)
+                 (pushback (send robot :angle-vector) avs)
+                 (send robot :angle-vector prev-av)
+                 (send self :angle-vector-sequence-raw avs)
+                 (send self :wait-interpolation)))))))
   (:gripper-servo-on
     (&optional (arm :arms))
     (dolist (l/r (if (eq arm :arms) (list "left" "right") (list (arm2str arm))))

From 76bea05602ad70a9fb5521f7feb01c19e560f768 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 19 Jun 2017 18:48:18 +0900
Subject: [PATCH 07/18] Translate palm end for good cylindrical grasp

---
 jsk_arc2017_baxter/euslisp/lib/baxter.l | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter.l b/jsk_arc2017_baxter/euslisp/lib/baxter.l
index 1a278b0a5..86fa0bd00 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter.l
@@ -51,6 +51,8 @@
                    ;; We have to move mimic joints, too
                    :right_gripper_r_finger_yaw_joint
                    :right_gripper_r_finger_roll_joint))
+    ;; translate palm end for good cylindrical grasp
+    (send self :rarm-palm-endpoint :translate #f(30 0 0) :local)
     )
   (:rarm-palm-endpoint
     (&rest args)

From 239e47953e533dcfa8fce1c4bba131d93c67c7c2 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 19:41:27 +0900
Subject: [PATCH 08/18] try-to-pick-object -> try-to-suction-object

---
 jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index c9b051d04..aa35e147b 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -270,7 +270,7 @@
               (send *ri* :move-hand arm
                     (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000))
             (setq graspingp
-                  (send self :try-to-pick-object arm obj-pos
+                  (send self :try-to-suction-object arm obj-pos
                         :offset (float-vector 0 0 (- (* i -50) 30))))
             (pushback (send *baxter* :angle-vector) avs))))
       (when do-stop-grasp (unless graspingp (send *ri* :stop-grasp arm)))
@@ -306,7 +306,7 @@
       (setq obj-box-z-length (z-of-cube (send self :bbox->cube obj-box)))
       (setq obj-pos (v+ obj-pos (float-vector 0 0 (/ obj-box-z-length 2))))
       obj-pos))
-  (:try-to-pick-object
+  (:try-to-suction-object
     (arm obj-pos &key (offset #f(0 0 0)))
     (let (graspingp)
       (if (eq arm :larm)
@@ -320,12 +320,12 @@
                 3000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
           ;; start the vacuum gripper after approaching to the object
-          (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm)
+          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
           (send *ri* :start-grasp arm)
           (unix::sleep 1))
         (progn
           ;; start the vacuum gripper before approaching to the object
-          (ros::ros-info "[:try-to-pick-object] arm:~a start vacuum gripper" arm)
+          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
           (send *ri* :start-grasp arm)
           (send *ri* :angle-vector-raw
                 (send *baxter* arm :inverse-kinematics
@@ -351,9 +351,9 @@
             3000 (send *ri* :get-arm-controller arm) 0)
       (send *ri* :wait-interpolation-until-grasp arm)
       (setq graspingp (send *ri* :graspingp arm))
-      (ros::ros-info "[:try-to-pick-object] arm:~a graspingp: ~a" arm graspingp)
+      (ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
       (unless graspingp
-        (ros::ros-info "[:try-to-pick-object] arm:~a again approach to the object" arm)
+        (ros::ros-info "[:try-to-suction-object] arm:~a again approach to the object" arm)
         (let ((temp-av (send *baxter* :angle-vector)))
           ;; only if robot can solve IK
           (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local)
@@ -368,14 +368,14 @@
         (send *ri* :move-hand arm
               (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000))
       ;; lift object
-      (ros::ros-info "[:try-to-pick-object] arm:~a lift the object" arm)
+      (ros::ros-info "[:try-to-suction-object] arm:~a lift the object" arm)
       (send *ri* :gripper-servo-off arm)
       (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world)
             3000 (send *ri* :get-arm-controller arm) 0)
       (send *ri* :wait-interpolation)
       (unix::sleep 1)  ;; wait for arm to follow
       (setq graspingp (send *ri* :graspingp arm))
-      (ros::ros-info "[:try-to-pick-object] arm:~a graspingp: ~a" arm graspingp)
+      (ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
     graspingp))
   (:ik->bin-center
     (arm bin &key (offset #f(0 0 0)) (rpy #f(0 0 0)) (rotation-axis t) (use-gripper nil))

From e856cc136361dcd545440b8339c04398f4053ee5 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sun, 18 Jun 2017 20:42:04 +0900
Subject: [PATCH 09/18] Add :try-to-pinch-object and use it

---
 .../euslisp/lib/arc-interface.l               | 119 ++++++++++++++----
 .../euslisp/lib/baxter-interface.l            |   9 +-
 .../euslisp/lib/pick-interface.l              |  57 +++++++--
 .../euslisp/lib/stow-interface.l              |  57 +++++++--
 4 files changed, 190 insertions(+), 52 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index aa35e147b..29741646f 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -265,22 +265,15 @@
       (dotimes (i n-trial)
         (dotimes (j n-trial-same-pos)
           (unless graspingp
-            (when (eq arm :rarm)
-              ;; Fold fingers again
-              (send *ri* :move-hand arm
-                    (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000))
             (setq graspingp
                   (send self :try-to-suction-object arm obj-pos
                         :offset (float-vector 0 0 (- (* i -50) 30))))
-            (pushback (send *baxter* :angle-vector) avs))))
+            (if (and (eq arm :rarm) (null graspingp))
+              (setq graspingp
+                    (send self :try-to-pinch-object arm obj-pos
+                          :offset (float-vector 0 0 (- (* i -50) 30))))))))
       (when do-stop-grasp (unless graspingp (send *ri* :stop-grasp arm)))
-      (send *ri* :angle-vector-sequence (reverse avs)
-            :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
       (send *ri* :wait-interpolation)
-      (when (eq arm :rarm)
-        ;; Move fingers to initial pose
-        (send *ri* :move-hand arm
-              (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
       (send *ri* :gripper-servo-on arm)
       graspingp))
   (:get-object-position
@@ -308,7 +301,7 @@
       obj-pos))
   (:try-to-suction-object
     (arm obj-pos &key (offset #f(0 0 0)))
-    (let (graspingp)
+    (let (graspingp prev-av target-av pad-angle)
       (if (eq arm :larm)
         (progn
           (send *ri* :angle-vector-raw
@@ -324,10 +317,17 @@
           (send *ri* :start-grasp arm)
           (unix::sleep 1))
         (progn
-          ;; start the vacuum gripper before approaching to the object
-          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
-          (send *ri* :start-grasp arm)
-          (send *ri* :angle-vector-raw
+          (send *ri* :stop-grasp arm :hand)
+          (send *baxter* :angle-vector
+                (send *ri* :state :potentio-vector :wait-until-update t))
+          (send *ri* :angle-vector-raw (send *baxter* :slide-gripper arm 60 :relative nil)
+                :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0)
+          (send *ri* :wait-interpolation)
+          ;; Fold fingers again
+          (send *ri* :move-hand arm
+                (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000)
+          (setq prev-av (send *baxter* :angle-vector))
+          (setq target-av
                 (send *baxter* arm :inverse-kinematics
                       (make-coords :pos (v+ obj-pos offset)
                                    :rpy #f(0 0 0))
@@ -338,7 +338,17 @@
                       #'(lambda ()
                           (and
                             (< (send *baxter* arm :gripper-x :joint-angle) 110)
-                            (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))
+                            (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))
+          (setq pad-angle (send *baxter* arm :gripper-p :joint-angle))
+          (send *baxter* :angle-vector prev-av)
+          (send *baxter* arm :gripper-p :joint-angle pad-angle)
+          (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
+                :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0)
+          (send *ri* :wait-interpolation)
+          ;; start the vacuum gripper before approaching to the object
+          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
+          (send *ri* :start-grasp arm)
+          (send *ri* :angle-vector-raw (send *baxter* :angle-vector target-av)
                 3000 (send *ri* :get-arm-controller arm) 0)
           ;; Wait until grasp to prevent tooth flying
           (send *ri* :wait-interpolation-until-grasp arm)))
@@ -377,8 +387,66 @@
       (setq graspingp (send *ri* :graspingp arm))
       (ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
     graspingp))
+  (:try-to-pinch-object
+    (arm obj-pos &key (offset #f(0 0 0)))
+    (let (graspingp)
+      ;; Initialize finger
+      (send *ri* :move-hand arm
+            (send *baxter* :hand-grasp-pose arm :cylindrical :angle 30) 1000)
+      ;; Initialize arm
+      (send *baxter* :rotate-gripper :rarm 0 :relative nil)
+      (send *baxter* :slide-gripper :rarm 60 :relative nil)
+      (send *ri* :angle-vector
+            (send *baxter* arm :inverse-kinematics
+                  (make-coords :pos (v+ obj-pos #f(0 0 200))
+                               :rpy (float-vector pi/2 0 0))
+                  :move-palm-end t
+                  :rotation-axis t)
+            :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
+      (send *ri* :wait-interpolation)
+      ;; start the vacuum gripper before approaching to the object
+      (ros::ros-info "[:try-to-pinch-object] arm:~a start vacuum gripper" arm)
+      (send *ri* :start-grasp arm)
+      (send *ri* :angle-vector-raw
+            (send *baxter* arm :inverse-kinematics
+                  (make-coords :pos (v+ obj-pos offset)
+                               :rpy (float-vector pi/2 0 0))
+                  :move-palm-end t
+                  :rotation-axis t)
+            3000 (send *ri* :get-arm-controller arm) 0)
+      ;; Wait until grasp or finger touch
+      (send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
+      (send *ri* :angle-vector-raw
+            (send *baxter* arm :inverse-kinematics
+                  (make-coords :pos obj-pos
+                               :rpy (float-vector pi/2 0 0))
+                  :move-palm-end t
+                  :rotation-axis t)
+            3000 (send *ri* :get-arm-controller arm) 0)
+      (send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
+      (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
+      (send *ri* :angle-vector-raw
+            (send *baxter* :slide-gripper :rarm 120 :relative nil)
+            3000 (send *ri* :get-arm-controller arm) 0)
+      (send *ri* :wait-interpolation-until arm :grasp)
+      (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
+      (send *ri* :start-grasp arm :hand)
+      (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
+      (setq graspingp (send *ri* :graspingp arm))
+      (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp)
+      ;; lift object
+      (ros::ros-info "[:try-to-pinch-object] arm:~a lift the object" arm)
+      (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 200) :world)
+            3000 (send *ri* :get-arm-controller arm
+                       :gripper (not (send *ri* :finger-closep arm))) 0)
+      (send *ri* :wait-interpolation)
+      (unix::sleep 1)  ;; wait for arm to follow
+      (setq graspingp (send *ri* :graspingp arm))
+      (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp)
+      graspingp))
   (:ik->bin-center
-    (arm bin &key (offset #f(0 0 0)) (rpy #f(0 0 0)) (rotation-axis t) (use-gripper nil))
+    (arm bin &key (offset #f(0 0 0)) (rpy #f(0 0 0))
+         (rotation-axis t) (use-gripper nil) (move-palm-end nil))
     (let (bin-cube bin-coords)
       (setq bin-cube (gethash bin bin-cubes-))
       (setq bin-coords (send bin-cube :copy-worldcoords))
@@ -391,9 +459,11 @@
       (send bin-coords :rotate (aref rpy 2) :x)
       (send *baxter* arm :inverse-kinematics bin-coords
             :rotation-axis rotation-axis
-            :use-gripper use-gripper)))
+            :use-gripper use-gripper
+            :move-palm-end move-palm-end)))
   (:ik->cardboard-center
-    (arm cardboard &key (offset #f(0 0 0)) (rotation-axis t) (use-gripper nil))
+    (arm cardboard &key (offset #f(0 0 0))
+         (rotation-axis t) (use-gripper nil) (move-palm-end nil))
     (send *baxter* arm :inverse-kinematics
           (make-coords
             :pos (v+ offset
@@ -402,9 +472,11 @@
                            (t (float-vector 800 -500 0))))
             :rpy (float-vector 0 0 0))
           :rotation-axis rotation-axis
-          :use-gripper use-gripper))
+          :use-gripper use-gripper
+          :move-palm-end move-palm-end))
   (:ik->tote-center
-    (arm &key (offset #f(0 0 0)) (rpy #f(0 0 0)) (rotation-axis t) (use-gripper nil))
+    (arm &key (offset #f(0 0 0)) (rpy #f(0 0 0))
+         (rotation-axis t) (use-gripper nil) (move-palm-end nil))
     (let (tote-cube tote-coords)
       (setq tote-cube (gethash arm tote-cubes-))
       (setq tote-coords (send tote-cube :copy-worldcoords))
@@ -417,7 +489,8 @@
       (send tote-coords :rotate (aref rpy 2) :x)
       (send *baxter* arm :inverse-kinematics tote-coords
             :rotation-axis rotation-axis
-            :use-gripper use-gripper)))
+            :use-gripper use-gripper
+            :move-palm-end move-palm-end)))
   (:move-arm-body->bin-overlook-pose
     (arm bin &key (gripper-angle 90))
     (let (avs offset rpy
diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index a46da39bc..31be272c1 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -198,10 +198,13 @@
             :joint-angle)
       (send robot (str2symbol (format nil "~a_gripper_finger_roll_joint" (arm2str arm)))
             :joint-angle)))
+  (:finger-closep (arm)
+    (if (eq arm :rarm)
+      (> (aref (send *ri* :get-real-finger-av arm) 1) 30) nil))
 
-  (:get-arm-controller (arm)
-    (cond ((eq arm :rarm) :rarm-controller)
-          ((eq arm :larm) :larm-controller)
+  (:get-arm-controller (arm &key (gripper t))
+    (cond ((eq arm :rarm) (if gripper :rarm-controller :rarm-no-gripper-controller))
+          ((eq arm :larm) (if gripper :larm-controller :larm-no-gripper-controller))
           (t nil)))
   (:start-grasp
     (&optional (arm :arms) (type :suction))
diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index d6596d5e3..d87dbc09b 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -108,16 +108,25 @@
           :n-trial 2
           :n-trial-same-pos 1
           :do-stop-grasp nil)
-    (send *ri* :angle-vector
-          (send self :ik->bin-center arm target-bin
-                :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
-          3000 (send *ri* :get-arm-controller arm) 0)
+    (if (send *ri* :finger-closep arm)
+      (send *ri* :angle-vector
+            (send self :ik->bin-center arm target-bin
+                  :offset #f(0 0 300) :rotation-axis :z :move-palm-end t)
+            3000 (send *ri* :get-arm-controller arm :gripper nil) 0)
+      (send *ri* :angle-vector
+            (send self :ik->bin-center arm target-bin
+                  :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
+            3000 (send *ri* :get-arm-controller arm) 0))
     (send *ri* :wait-interpolation)
     (setq graspingp (send *ri* :graspingp arm))
     (ros::ros-info "[main] arm: ~a graspingp: ~a" arm graspingp)
     graspingp)
   (:return-from-pick-object (arm)
-    (send *ri* :stop-grasp arm)
+    (send *ri* :stop-grasp arm :suction)
+    (when (eq arm :rarm)
+      (send *ri* :stop-grasp arm :hand)
+      (send *ri* :move-hand arm
+            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (send *ri* :angle-vector-sequence
           (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f))
                 (send *baxter* :fold-pose-back arm))
@@ -161,28 +170,50 @@
       (ros::ros-info "[main] ~a, place object in bin ~a" arm target-cardboard)
       (when moveit-p (send self :add-shelf-scene))
       (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 80 -80))
-      (send *ri* :angle-vector
-            (send self :ik->cardboard-center arm target-cardboard
-                  :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
-            4000 (send *ri* :get-arm-controller arm) 0)
+      (if (send *ri* :finger-closep arm)
+        (send *ri* :angle-vector
+              (send self :ik->cardboard-center arm target-cardboard
+                    :offset #f(0 0 300) :rotation-axis nil :move-palm-end t)
+              4000 (send *ri* :get-arm-controller arm :gripper nil) 0)
+        (progn
+          (if (eq arm :rarm)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
+          (send *ri* :angle-vector
+                (send self :ik->cardboard-center arm target-cardboard
+                      :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
+                4000 (send *ri* :get-arm-controller arm) 0)))
       (send *ri* :wait-interpolation)
       (setq dropped (not (send *ri* :graspingp arm)))
       (if dropped
         (progn
           (ros::ros-error "[main] arm ~a: dropped object" arm)
-          (send *ri* :stop-grasp arm))
+          (send *ri* :stop-grasp arm :suction)
+          (when (eq arm :rarm)
+            (send *ri* :stop-grasp arm :hand)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
         (progn
           (ros::ros-info-green "[main] arm ~a: place object ~a in cardboard ~a" arm target-obj target-cardboard)
           (when moveit-p (send self :delete-cardboard-scene arm))
-          (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 -300) :world)
+          (send *ri* :angle-vector-raw
+                (send *baxter* arm :move-end-pos #f(0 0 -300) :world
+                      :rotation-axis (not (send *ri* :finger-closep arm)))
                 2000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
-          (send *ri* :stop-grasp arm) ;; release object
+          (send *ri* :stop-grasp arm :suction) ;; release object
+          (if (eq arm :rarm)
+            (send *ri* :stop-grasp arm :hand)) ;; release object
           (send self :spin-off-by-wrist arm :times 10)
           (send *ri* :wait-interpolation)
-          (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 300) :world)
+          (send *ri* :angle-vector-raw
+                (send *baxter* arm :move-end-pos #f(0 0 300) :world
+                      :rotation-axis (not (send *ri* :finger-closep arm)))
                 1000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
+          (if (eq arm :rarm)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
           (when moveit-p (send self :add-cardboard-scene arm))))))
   (:return-from-place-object (arm)
     (send self :fold-pose-back arm)
diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
index 4f489d67d..9cb00c75b 100644
--- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
@@ -78,16 +78,25 @@
           :n-trial 2
           :n-trial-same-pos 1
           :do-stop-grasp nil)
-    (send *ri* :angle-vector
-          (send self :ik->tote-center arm
-                :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
-          3000 (send *ri* :get-arm-controller arm) 0)
+    (if (send *ri* :finger-closep arm)
+      (send *ri* :angle-vector
+            (send self :ik->tote-center arm
+                  :offset #f(0 0 300) :rotation-axis :z :move-palm-end t)
+            3000 (send *ri* :get-arm-controller arm :gripper nil) 0)
+      (send *ri* :angle-vector
+            (send self :ik->tote-center arm
+                  :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
+            3000 (send *ri* :get-arm-controller arm) 0))
     (send *ri* :wait-interpolation)
     (setq graspingp (send *ri* :graspingp arm))
     (ros::ros-info "[main] arm: ~a graspingp: ~a" arm graspingp)
     graspingp)
   (:return-from-pick-object (arm)
-    (send *ri* :stop-grasp arm)
+    (send *ri* :stop-grasp arm :suction)
+    (when (eq arm :rarm)
+      (send *ri* :stop-grasp arm :hand)
+      (send *ri* :move-hand arm
+            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (ros::ros-info "[main] arm: ~a return from pick-object to fold-pose-back" arm)
     (send *ri* :angle-vector-sequence
           (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f))
@@ -135,28 +144,50 @@
       (ros::ros-info "[main] ~a, place object in bin ~a" arm target-bin)
       (when moveit-p (send self :add-tote-scene arm))
       (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 80 -80))
-      (send *ri* :angle-vector
-            (send self :ik->bin-center arm target-bin
-                  :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
-            4000 (send *ri* :get-arm-controller arm) 0)
+      (if (send *ri* :finger-closep arm)
+        (send *ri* :angle-vector
+              (send self :ik->bin-center arm target-bin
+                    :offset #f(0 0 300) :rotation-axis nil :move-palm-end t)
+              4000 (send *ri* :get-arm-controller arm :gripper nil) 0)
+        (progn
+          (if (eq arm :rarm)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
+          (send *ri* :angle-vector
+                (send self :ik->bin-center arm target-bin
+                      :offset #f(0 0 300) :rotation-axis :z :use-gripper t)
+                4000 (send *ri* :get-arm-controller arm) 0)))
       (send *ri* :wait-interpolation)
       (setq dropped (not (send *ri* :graspingp arm)))
       (if dropped
         (progn
           (ros::ros-error "[main] arm ~a: dropped object" arm)
-          (send *ri* :stop-grasp arm))
+          (send *ri* :stop-grasp arm :suction)
+          (when (eq arm :rarm)
+            (send *ri* :stop-grasp arm :hand)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
         (progn
           (ros::ros-info-green "[main] arm ~a: place object ~a in bin ~a" arm target-obj target-bin)
           (when moveit-p (send self :delete-bin-scene target-bin))
-          (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 -300) :world)
+          (send *ri* :angle-vector-raw
+                (send *baxter* arm :move-end-pos #f(0 0 -300) :world
+                      :rotation-axis (not (send *ri* :finger-closep arm)))
                 2000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
-          (send *ri* :stop-grasp arm) ;; release object
+          (send *ri* :stop-grasp arm :suction) ;; release object
+          (if (eq arm :rarm)
+            (send *ri* :stop-grasp arm :hand)) ;; release object
           (send self :spin-off-by-wrist arm :times 10)
           (send *ri* :wait-interpolation)
-          (send *ri* :angle-vector-raw (send *baxter* arm :move-end-pos #f(0 0 300) :world)
+          (send *ri* :angle-vector-raw
+                (send *baxter* arm :move-end-pos #f(0 0 300) :world
+                      :rotation-axis (not (send *ri* :finger-closep arm)))
                 1000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
+          (if (eq arm :rarm)
+            (send *ri* :move-hand arm
+                  (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
           (when moveit-p (send self :add-bin-scene target-bin))))))
   (:return-from-place-object (arm)
     (send self :fold-pose-back arm)

From 0020f20a299b6c6ccf0cce2696ba3ce779f3055f Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 20 Jun 2017 10:23:04 +0900
Subject: [PATCH 10/18] Add :prismatic-loaded in :wait-interpolation-until

---
 .../euslisp/lib/baxter-interface.l              | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index 31be272c1..2810a6d3c 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -71,6 +71,7 @@
     (setq finger-load- (make-hash-table))
     (setq finger-flex- (make-hash-table))
     (setq prismatic-load- (make-hash-table))
+    (setq prismatic-vel- (make-hash-table))
     (dolist (arm (list :rarm :larm))
       (ros::subscribe (format nil "/gripper_front/limb/~a/pressure/state" (arm2str arm))
                       std_msgs::Float64
@@ -84,7 +85,7 @@
       (ros::subscribe (format nil "/gripper_front/limb/~a/dxl/prismatic_joint_controller/state"
                               (arm2str arm))
                       dynamixel_msgs::JointState
-                      #'send self :set-prismatic-load arm
+                      #'send self :set-prismatic-state arm
                       :groupname groupname)
       (sethash arm finger-flex- (make-hash-table))
       (dolist (side (list :right :left))
@@ -145,9 +146,10 @@
   (:set-finger-load
     (arm msg)
     (sethash arm finger-load- (send msg :load)))
-  (:set-prismatic-load
+  (:set-prismatic-state
     (arm msg)
-    (sethash arm prismatic-load- (send msg :load)))
+    (sethash arm prismatic-load- (send msg :load))
+    (sethash arm prismatic-vel- (send msg :velocity)))
   (:set-finger-flex
     (arm side msg)
     (sethash side (gethash arm finger-flex-) (send msg :data)))
@@ -191,6 +193,9 @@
   (:get-prismatic-load (arm)
     (send self :spin-once)
     (gethash arm prismatic-load-))
+  (:get-prismatic-vel (arm)
+    (send self :spin-once)
+    (gethash arm prismatic-vel-))
   (:get-real-finger-av (arm)
     (send self :update-robot-state :wait-until-update t)
     (float-vector
@@ -355,6 +360,12 @@
         (pushback
           #'(lambda () (< (send self :get-finger-load arm) (- init-load 0.01)))
           conds))
+      (when (member :prismatic-loaded args)
+        (ros::ros-info "[:wait-interpolation-until] Prepare for prismatic joint loaded")
+        (pushback
+          #'(lambda () (and (< (send self :get-prismatic-load arm) -0.07)
+                            (< (send self :get-prismatic-vel arm) 0.01)))
+          conds))
       ;; wait for :interpolatingp
       (unix::usleep 1000)
       (while (send self :interpolatingp)

From 7db4bd339a3678db5fba5591ae3af840d6f9594e Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 20 Jun 2017 11:21:01 +0900
Subject: [PATCH 11/18] Use prismatic-loaded to detect pushing object

---
 .../euslisp/lib/arc-interface.l               | 24 ++++++++++++-------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index 29741646f..7f4544082 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -350,8 +350,8 @@
           (send *ri* :start-grasp arm)
           (send *ri* :angle-vector-raw (send *baxter* :angle-vector target-av)
                 3000 (send *ri* :get-arm-controller arm) 0)
-          ;; Wait until grasp to prevent tooth flying
-          (send *ri* :wait-interpolation-until-grasp arm)))
+          ;; Wait until grasp or push object to prevent tooth flying
+          (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)))
       (send *ri* :angle-vector-raw
             (send *baxter* arm :inverse-kinematics
                   (make-coords :pos obj-pos
@@ -359,7 +359,9 @@
                   :use-gripper t
                   :rotation-axis :z)
             3000 (send *ri* :get-arm-controller arm) 0)
-      (send *ri* :wait-interpolation-until-grasp arm)
+      (if (eq arm :larm)
+        (send *ri* :wait-interpolation-until-grasp arm)
+        (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))
       (setq graspingp (send *ri* :graspingp arm))
       (ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
       (unless graspingp
@@ -369,10 +371,14 @@
           (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local)
             (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
                   3000 (send *ri* :get-arm-controller arm) 0))
-          (send *ri* :wait-interpolation-until-grasp arm)
+          (if (eq arm :larm)
+            (send *ri* :wait-interpolation-until-grasp arm)
+            (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))
           (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av)
                   3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter
-          (send *ri* :wait-interpolation-until-grasp arm)))
+          (if (eq arm :larm)
+            (send *ri* :wait-interpolation-until-grasp arm)
+            (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))))
       (when (eq arm :rarm)
         ;; Open fingers in bin
         (send *ri* :move-hand arm
@@ -415,7 +421,8 @@
                   :rotation-axis t)
             3000 (send *ri* :get-arm-controller arm) 0)
       ;; Wait until grasp or finger touch
-      (send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
+      (send *ri* :wait-interpolation-until arm
+            :grasp :finger-flexion :finger-loaded :prismatic-loaded)
       (send *ri* :angle-vector-raw
             (send *baxter* arm :inverse-kinematics
                   (make-coords :pos obj-pos
@@ -423,12 +430,13 @@
                   :move-palm-end t
                   :rotation-axis t)
             3000 (send *ri* :get-arm-controller arm) 0)
-      (send *ri* :wait-interpolation-until arm :grasp :finger-flexion :finger-loaded)
+      (send *ri* :wait-interpolation-until arm
+            :grasp :finger-flexion :finger-loaded :prismatic-loaded)
       (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
       (send *ri* :angle-vector-raw
             (send *baxter* :slide-gripper :rarm 120 :relative nil)
             3000 (send *ri* :get-arm-controller arm) 0)
-      (send *ri* :wait-interpolation-until arm :grasp)
+      (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)
       (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
       (send *ri* :start-grasp arm :hand)
       (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))

From 9b1735960b111f591522d7f634260dfb156a641e Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 20 Jun 2017 22:04:19 +0900
Subject: [PATCH 12/18] Add set-grasp-style state in stow

---
 .../euslisp/lib/arc-interface.l               | 111 ++++++++----------
 .../euslisp/lib/stow-interface.l              |  27 +++--
 jsk_arc2017_baxter/euslisp/stow-main.l        |   8 +-
 .../node_scripts/state_server.py              |   3 +-
 4 files changed, 78 insertions(+), 71 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index 7f4544082..27731dac4 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -213,7 +213,8 @@
       (setq graspingp (send* self :pick-object-with-movable-region arm movable-region args))
       graspingp))
   (:pick-object-with-movable-region
-    (arm movable-region &key (n-trial 1) (n-trial-same-pos 1) (do-stop-grasp nil))
+    (arm movable-region &key (n-trial 1) (n-trial-same-pos 1)
+         (do-stop-grasp nil) (grasp-style :suction))
     (let (graspingp avs object-index)
       ;; TODO: object-index is set randomly
       (setq object-index (random (length (gethash arm object-boxes-))))
@@ -233,18 +234,28 @@
           (send *ri* :wait-interpolation))
         (let (prev-av next-av gripper-x)
           (setq prev-av (send *baxter* :angle-vector))
-          (setq next-av
-                (send *baxter* arm :inverse-kinematics
-                      (make-coords :pos (v+ obj-pos #f(0 0 150))
-                                   :rpy #f(0 0 0))
-                      :use-gripper t
-                      :rotation-axis :z
-                      :stop 200
-                      :additional-check
-                      #'(lambda ()
-                          (and
-                            (< (send *baxter* arm :gripper-x :joint-angle) 110)
-                            (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))
+          (if (eq grasp-style :suction)
+            (setq next-av
+                  (send *baxter* arm :inverse-kinematics
+                        (make-coords :pos (v+ obj-pos #f(0 0 150))
+                                     :rpy #f(0 0 0))
+                        :use-gripper t
+                        :rotation-axis :z
+                        :stop 200
+                        :additional-check
+                        #'(lambda ()
+                            (and
+                              (< (send *baxter* arm :gripper-x :joint-angle) 110)
+                              (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))
+            (progn
+              (send *baxter* :rotate-gripper :rarm 0 :relative nil)
+              (send *baxter* :slide-gripper :rarm 60 :relative nil)
+              (setq next-av
+                    (send *baxter* arm :inverse-kinematics
+                          (make-coords :pos (v+ obj-pos #f(0 0 200))
+                                       :rpy (float-vector pi/2 0 0))
+                          :move-palm-end t
+                          :rotation-axis t))))
           (setq gripper-x (send *baxter* arm :gripper-x :joint-angle))
           (send *baxter* :angle-vector prev-av)
           ;; First, move prismatic joint to target position
@@ -266,10 +277,9 @@
         (dotimes (j n-trial-same-pos)
           (unless graspingp
             (setq graspingp
-                  (send self :try-to-suction-object arm obj-pos
-                        :offset (float-vector 0 0 (- (* i -50) 30))))
-            (if (and (eq arm :rarm) (null graspingp))
-              (setq graspingp
+                  (if (eq grasp-style :suction)
+                    (send self :try-to-suction-object arm obj-pos
+                          :offset (float-vector 0 0 (- (* i -50) 30)))
                     (send self :try-to-pinch-object arm obj-pos
                           :offset (float-vector 0 0 (- (* i -50) 30))))))))
       (when do-stop-grasp (unless graspingp (send *ri* :stop-grasp arm)))
@@ -301,7 +311,7 @@
       obj-pos))
   (:try-to-suction-object
     (arm obj-pos &key (offset #f(0 0 0)))
-    (let (graspingp prev-av target-av pad-angle)
+    (let (graspingp)
       (if (eq arm :larm)
         (progn
           (send *ri* :angle-vector-raw
@@ -317,17 +327,13 @@
           (send *ri* :start-grasp arm)
           (unix::sleep 1))
         (progn
-          (send *ri* :stop-grasp arm :hand)
-          (send *baxter* :angle-vector
-                (send *ri* :state :potentio-vector :wait-until-update t))
-          (send *ri* :angle-vector-raw (send *baxter* :slide-gripper arm 60 :relative nil)
-                :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0)
-          (send *ri* :wait-interpolation)
           ;; Fold fingers again
           (send *ri* :move-hand arm
                 (send *baxter* :hand-grasp-pose arm :cylindrical :angle 100) 1000)
-          (setq prev-av (send *baxter* :angle-vector))
-          (setq target-av
+          ;; start the vacuum gripper before approaching to the object
+          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
+          (send *ri* :start-grasp arm)
+          (send *ri* :angle-vector-raw
                 (send *baxter* arm :inverse-kinematics
                       (make-coords :pos (v+ obj-pos offset)
                                    :rpy #f(0 0 0))
@@ -338,17 +344,7 @@
                       #'(lambda ()
                           (and
                             (< (send *baxter* arm :gripper-x :joint-angle) 110)
-                            (< -30 (send *baxter* arm :gripper-p :joint-angle) 30)))))
-          (setq pad-angle (send *baxter* arm :gripper-p :joint-angle))
-          (send *baxter* :angle-vector prev-av)
-          (send *baxter* arm :gripper-p :joint-angle pad-angle)
-          (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
-                :fast (send *ri* :get-arm-controller arm) 0 :scale 1.0)
-          (send *ri* :wait-interpolation)
-          ;; start the vacuum gripper before approaching to the object
-          (ros::ros-info "[:try-to-suction-object] arm:~a start vacuum gripper" arm)
-          (send *ri* :start-grasp arm)
-          (send *ri* :angle-vector-raw (send *baxter* :angle-vector target-av)
+                            (< -30 (send *baxter* arm :gripper-p :joint-angle) 30))))
                 3000 (send *ri* :get-arm-controller arm) 0)
           ;; Wait until grasp or push object to prevent tooth flying
           (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)))
@@ -367,18 +363,26 @@
       (unless graspingp
         (ros::ros-info "[:try-to-suction-object] arm:~a again approach to the object" arm)
         (let ((temp-av (send *baxter* :angle-vector)))
-          ;; only if robot can solve IK
-          (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local)
-            (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
-                  3000 (send *ri* :get-arm-controller arm) 0))
-          (if (eq arm :larm)
-            (send *ri* :wait-interpolation-until-grasp arm)
-            (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))
-          (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av)
-                  3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter
           (if (eq arm :larm)
-            (send *ri* :wait-interpolation-until-grasp arm)
-            (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded))))
+            (progn
+              ;; only if robot can solve IK
+              (if (send *baxter* arm :move-end-pos #f(0 0 -50) :local)
+                (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
+                      3000 (send *ri* :get-arm-controller arm) 0))
+              (send *ri* :wait-interpolation-until-grasp arm)
+              (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av)
+                    3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter
+              (send *ri* :wait-interpolation-until-grasp arm)
+              )
+            (progn
+              ;; only if robot can solve IK
+              (if (send *baxter* arm :move-end-pos #f(0 0 -100) :local)
+                (send *ri* :angle-vector-raw (send *baxter* :angle-vector)
+                      3000 (send *ri* :get-arm-controller arm) 0))
+              (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)
+              (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av)
+                    3000 (send *ri* :get-arm-controller arm) 0) ;; revert baxter
+              (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)))))
       (when (eq arm :rarm)
         ;; Open fingers in bin
         (send *ri* :move-hand arm
@@ -399,17 +403,6 @@
       ;; Initialize finger
       (send *ri* :move-hand arm
             (send *baxter* :hand-grasp-pose arm :cylindrical :angle 30) 1000)
-      ;; Initialize arm
-      (send *baxter* :rotate-gripper :rarm 0 :relative nil)
-      (send *baxter* :slide-gripper :rarm 60 :relative nil)
-      (send *ri* :angle-vector
-            (send *baxter* arm :inverse-kinematics
-                  (make-coords :pos (v+ obj-pos #f(0 0 200))
-                               :rpy (float-vector pi/2 0 0))
-                  :move-palm-end t
-                  :rotation-axis t)
-            :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
-      (send *ri* :wait-interpolation)
       ;; start the vacuum gripper before approaching to the object
       (ros::ros-info "[:try-to-pinch-object] arm:~a start vacuum gripper" arm)
       (send *ri* :start-grasp arm)
diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
index 9cb00c75b..0e3c47561 100644
--- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
@@ -12,19 +12,21 @@
 (defclass jsk_arc2017_baxter::stow-interface
   :super jsk_arc2017_baxter::arc-interface
   :slots (graspingp
-           moveit-p
+          moveit-p
           picking-fail-count
           recognize-fail-count
           target-bin
           target-obj
-          tote-contents))
+          tote-contents
+          grasp-style))
 
 (defmethod jsk_arc2017_baxter::stow-interface
   (:init (&key (moveit nil))
     (send-super :init)
     (setq picking-fail-count 0)
     (setq recognize-fail-count 0)
-    (setq moveit-p moveit))
+    (setq moveit-p moveit)
+    (setq grasp-style nil))
   (:recognize-bboxes (arm)
     (ros::ros-info "[main] recognizing bin boxes")
     (send self :recognize-bin-boxes :stamp (ros::time-now))
@@ -71,14 +73,18 @@
     (incf recognize-fail-count)
     (ros::ros-info "[main] arm: ~a, recognize fail count: ~a" arm recognize-fail-count)
     (> recognize-fail-count 1))
+  (:set-grasp-style (arm)
+    (if (and (eq grasp-style :suction) (eq arm :rarm))
+      (setq grasp-style :pinch) (setq grasp-style :suction)))
   (:pick-object (arm)
     (setq recognize-fail-count 0)
     (when moveit-p (send self :delete-tote-scene arm))
     (send self :pick-object-in-tote arm
-          :n-trial 2
+          :n-trial 1
           :n-trial-same-pos 1
-          :do-stop-grasp nil)
-    (if (send *ri* :finger-closep arm)
+          :do-stop-grasp nil
+          :grasp-style grasp-style)
+    (if (eq grasp-style :pinch)
       (send *ri* :angle-vector
             (send self :ik->tote-center arm
                   :offset #f(0 0 300) :rotation-axis :z :move-palm-end t)
@@ -144,7 +150,7 @@
       (ros::ros-info "[main] ~a, place object in bin ~a" arm target-bin)
       (when moveit-p (send self :add-tote-scene arm))
       (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 80 -80))
-      (if (send *ri* :finger-closep arm)
+      (if (eq grasp-style :pinch)
         (send *ri* :angle-vector
               (send self :ik->bin-center arm target-bin
                     :offset #f(0 0 300) :rotation-axis nil :move-palm-end t)
@@ -172,7 +178,7 @@
           (when moveit-p (send self :delete-bin-scene target-bin))
           (send *ri* :angle-vector-raw
                 (send *baxter* arm :move-end-pos #f(0 0 -300) :world
-                      :rotation-axis (not (send *ri* :finger-closep arm)))
+                      :rotation-axis (eq grasp-style :suction))
                 2000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
           (send *ri* :stop-grasp arm :suction) ;; release object
@@ -182,13 +188,14 @@
           (send *ri* :wait-interpolation)
           (send *ri* :angle-vector-raw
                 (send *baxter* arm :move-end-pos #f(0 0 300) :world
-                      :rotation-axis (not (send *ri* :finger-closep arm)))
+                      :rotation-axis (eq grasp-style :suction))
                 1000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
           (if (eq arm :rarm)
             (send *ri* :move-hand arm
                   (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
-          (when moveit-p (send self :add-bin-scene target-bin))))))
+          (when moveit-p (send self :add-bin-scene target-bin))))
+      (setq grasp-style nil)))
   (:return-from-place-object (arm)
     (send self :fold-pose-back arm)
     (send *ri* :wait-interpolation)))
diff --git a/jsk_arc2017_baxter/euslisp/stow-main.l b/jsk_arc2017_baxter/euslisp/stow-main.l
index d5af912e2..7d94bc763 100755
--- a/jsk_arc2017_baxter/euslisp/stow-main.l
+++ b/jsk_arc2017_baxter/euslisp/stow-main.l
@@ -17,11 +17,12 @@
             (:wait-for-user-input !-> :finish)
             (:wait-for-opposite-arm -> :recognize-object)
             (:wait-for-opposite-arm !-> :wait-for-opposite-arm)
-            (:recognize-object -> :pick-object)
+            (:recognize-object -> :set-grasp-style)
             (:recognize-object !-> :return-from-recognize-object)
             (:return-from-recognize-object -> :check-recognize-fail-count)
             (:check-recognize-fail-count -> :wait-for-user-input)
             (:check-recognize-fail-count !-> :wait-for-opposite-arm-start-picking)
+            (:set-grasp-style -> :pick-object)
             (:pick-object -> :verify-object)
             (:pick-object !-> :return-from-pick-object)
             (:return-from-pick-object -> :check-picking-fail-count)
@@ -68,6 +69,11 @@
               '(lambda (userdata)
                  (send *ti* :update-state *arm* :check-recognize-fail-count)
                  (send *ti* :check-recognize-fail-count *arm*)))
+            (:set-grasp-style
+              '(lambda (userdata)
+                 (send *ti* :update-state *arm* :set-grasp-style)
+                 (send *ti* :set-grasp-style *arm*)
+                 t))
             (:pick-object
               '(lambda (userdata)
                  (send *ti* :update-state *arm* :pick-object)
diff --git a/jsk_arc2017_baxter/node_scripts/state_server.py b/jsk_arc2017_baxter/node_scripts/state_server.py
index 2267512a5..377fed7af 100755
--- a/jsk_arc2017_baxter/node_scripts/state_server.py
+++ b/jsk_arc2017_baxter/node_scripts/state_server.py
@@ -104,7 +104,7 @@ def _get_can_start(self, state, opposite_state):
             else:
                 can_start = True
         else:
-            # pick task: wait condition
+            # stow task: wait condition
             if state == 'wait-for-opposite-arm-start-picking':
                 if opposite_state == 'recognize-object' \
                         or opposite_state == 'wait-for-user-input':
@@ -113,6 +113,7 @@ def _get_can_start(self, state, opposite_state):
                     can_start = False
             elif state == 'wait-for-opposite-arm':
                 if opposite_state == 'recognize-object' \
+                        or opposite_state == 'set-grasp-style' \
                         or opposite_state == 'pick-object' \
                         or opposite_state == 'verify-object' \
                         or opposite_state == 'set-target-bin' \

From 353cc0753e68d0c1414ef468638b19e236fc6dea Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Wed, 21 Jun 2017 01:53:19 +0900
Subject: [PATCH 13/18] Add set-grasp-style state in pick

---
 .../euslisp/lib/pick-interface.l              | 26 ++++++++++++-------
 jsk_arc2017_baxter/euslisp/pick-main.l        |  8 +++++-
 .../node_scripts/state_server.py              |  1 +
 3 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index d87dbc09b..84e0434f2 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -21,14 +21,16 @@
           order
           target-bin
           target-cardboard
-          target-obj))
+          target-obj
+          grasp-style))
 
 (defmethod jsk_arc2017_baxter::pick-interface
   (:init (&key (moveit nil))
     (send-super :init)
     (setq picking-fail-count 0)
     (setq bins (list :a :b :c))
-    (setq moveit-p moveit))
+    (setq moveit-p moveit)
+    (setq grasp-style nil))
   (:recognize-bin-boxes ()
     (ros::ros-info "[main] recognizing shelf bin boxes")
     (send-super :recognize-bin-boxes :stamp (ros::time-now))
@@ -46,6 +48,7 @@
       (setq order nil)
       can-start))
   (:set-target (arm)
+    (if (oddp picking-fail-count) (return-from :set-target t))
     (setq label-names
           (ros::get-param (format nil "/~a_hand_camera/label_names"
                                   (arm2str arm))))
@@ -102,13 +105,17 @@
     (send *ri* :wait-interpolation)
     (when moveit-p (send self :delete-shelf-scene))
     (when moveit-p (send self :delete-cardboard-scene arm)))
+  (:set-grasp-style (arm)
+    (if (and (eq grasp-style :suction) (eq arm :rarm))
+      (setq grasp-style :pinch) (setq grasp-style :suction)))
   (:pick-object (arm)
     (when moveit-p (send self :delete-shelf-scene))
     (send self :pick-object-in-bin arm target-bin
-          :n-trial 2
+          :n-trial 1
           :n-trial-same-pos 1
-          :do-stop-grasp nil)
-    (if (send *ri* :finger-closep arm)
+          :do-stop-grasp nil
+          :grasp-style grasp-style)
+    (if (eq grasp-style :pinch)
       (send *ri* :angle-vector
             (send self :ik->bin-center arm target-bin
                   :offset #f(0 0 300) :rotation-axis :z :move-palm-end t)
@@ -170,7 +177,7 @@
       (ros::ros-info "[main] ~a, place object in bin ~a" arm target-cardboard)
       (when moveit-p (send self :add-shelf-scene))
       (send *baxter* :head_pan :joint-angle (if (eq arm :larm) 80 -80))
-      (if (send *ri* :finger-closep arm)
+      (if (eq grasp-style :pinch)
         (send *ri* :angle-vector
               (send self :ik->cardboard-center arm target-cardboard
                     :offset #f(0 0 300) :rotation-axis nil :move-palm-end t)
@@ -198,7 +205,7 @@
           (when moveit-p (send self :delete-cardboard-scene arm))
           (send *ri* :angle-vector-raw
                 (send *baxter* arm :move-end-pos #f(0 0 -300) :world
-                      :rotation-axis (not (send *ri* :finger-closep arm)))
+                      :rotation-axis (eq grasp-style :suction))
                 2000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
           (send *ri* :stop-grasp arm :suction) ;; release object
@@ -208,13 +215,14 @@
           (send *ri* :wait-interpolation)
           (send *ri* :angle-vector-raw
                 (send *baxter* arm :move-end-pos #f(0 0 300) :world
-                      :rotation-axis (not (send *ri* :finger-closep arm)))
+                      :rotation-axis (eq grasp-style :suction))
                 1000 (send *ri* :get-arm-controller arm) 0)
           (send *ri* :wait-interpolation)
           (if (eq arm :rarm)
             (send *ri* :move-hand arm
                   (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
-          (when moveit-p (send self :add-cardboard-scene arm))))))
+          (when moveit-p (send self :add-cardboard-scene arm))))
+      (setq grasp-style nil)))
   (:return-from-place-object (arm)
     (send self :fold-pose-back arm)
     (send *ri* :wait-interpolation)))
diff --git a/jsk_arc2017_baxter/euslisp/pick-main.l b/jsk_arc2017_baxter/euslisp/pick-main.l
index 0d18eee2c..b424a4ce9 100755
--- a/jsk_arc2017_baxter/euslisp/pick-main.l
+++ b/jsk_arc2017_baxter/euslisp/pick-main.l
@@ -19,9 +19,10 @@
             (:wait-for-opposite-arm !-> :wait-for-opposite-arm)
             (:set-target -> :recognize-object)
             (:set-target !-> :wait-for-user-input)
-            (:recognize-object -> :pick-object)
+            (:recognize-object -> :set-grasp-style)
             (:recognize-object !-> :return-from-recognize-object)
             (:return-from-recognize-object -> :wait-for-opposite-arm-start-picking)
+            (:set-grasp-style -> :pick-object)
             (:pick-object -> :verify-object)
             (:pick-object !-> :return-from-pick-object)
             (:return-from-pick-object -> :check-picking-fail-count)
@@ -68,6 +69,11 @@
                  (send *ti* :update-state *arm* :return-from-recognize-object)
                  (send *ti* :return-from-recognize-object *arm*)
                  t))
+            (:set-grasp-style
+              '(lambda (userdata)
+                 (send *ti* :update-state *arm* :set-grasp-style)
+                 (send *ti* :set-grasp-style *arm*)
+                 t))
             (:pick-object
               '(lambda (userdata)
                  (send *ti* :update-state *arm* :pick-object)
diff --git a/jsk_arc2017_baxter/node_scripts/state_server.py b/jsk_arc2017_baxter/node_scripts/state_server.py
index 377fed7af..71739d07c 100755
--- a/jsk_arc2017_baxter/node_scripts/state_server.py
+++ b/jsk_arc2017_baxter/node_scripts/state_server.py
@@ -94,6 +94,7 @@ def _get_can_start(self, state, opposite_state):
             elif state == 'wait-for-opposite-arm':
                 if opposite_state == 'set-target' \
                         or opposite_state == 'recognize-object' \
+                        or opposite_state == 'set-grasp-style' \
                         or opposite_state == 'pick-object' \
                         or opposite_state == 'verify-object' \
                         or opposite_state == 'set-target-cardboard' \

From 5ad8b0e791835c4414d21215c02e2afcbff2aad4 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Wed, 21 Jun 2017 10:51:10 +0900
Subject: [PATCH 14/18] Wait longer for :interpolatingp

---
 jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index 2810a6d3c..aa5eb4557 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -367,7 +367,7 @@
                             (< (send self :get-prismatic-vel arm) 0.01)))
           conds))
       ;; wait for :interpolatingp
-      (unix::usleep 1000)
+      (unix::usleep 10000)
       (while (send self :interpolatingp)
         (when (reduce #'(lambda (x y) (or x y)) (mapcar #'funcall conds))
           (ros::ros-info "[:wait-interpolation-until-grasp] Cancel angle vector: ~a" arm)

From 20a74f864ca6a04d026425dab67e6cabaa8de668 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Wed, 21 Jun 2017 11:33:09 +0900
Subject: [PATCH 15/18] Add check-grasp-style state in pick task

---
 jsk_arc2017_baxter/euslisp/lib/pick-interface.l | 3 ++-
 jsk_arc2017_baxter/euslisp/pick-main.l          | 8 +++++++-
 jsk_arc2017_baxter/node_scripts/state_server.py | 5 +++--
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index 84e0434f2..742c4cd46 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -47,8 +47,9 @@
       (ros::ros-info "[:wait-for-user-input] received user input: ~a" arm)
       (setq order nil)
       can-start))
+  (:check-grasp-style (arm)
+    (if (eq grasp-style :suction) nil t))
   (:set-target (arm)
-    (if (oddp picking-fail-count) (return-from :set-target t))
     (setq label-names
           (ros::get-param (format nil "/~a_hand_camera/label_names"
                                   (arm2str arm))))
diff --git a/jsk_arc2017_baxter/euslisp/pick-main.l b/jsk_arc2017_baxter/euslisp/pick-main.l
index b424a4ce9..ac0be78bc 100755
--- a/jsk_arc2017_baxter/euslisp/pick-main.l
+++ b/jsk_arc2017_baxter/euslisp/pick-main.l
@@ -15,8 +15,10 @@
             (:recognize-bin-boxes -> :wait-for-user-input)
             (:wait-for-user-input -> :wait-for-opposite-arm)
             (:wait-for-user-input !-> :finish)
-            (:wait-for-opposite-arm -> :set-target)
+            (:wait-for-opposite-arm -> :check-grasp-style)
             (:wait-for-opposite-arm !-> :wait-for-opposite-arm)
+            (:check-grasp-style -> :set-target)
+            (:check-grasp-style !-> :recognize-object)
             (:set-target -> :recognize-object)
             (:set-target !-> :wait-for-user-input)
             (:recognize-object -> :set-grasp-style)
@@ -56,6 +58,10 @@
                  (send *ti* :update-state *arm* :wait-for-opposite-arm)
                  (send *ti* :check-can-start *arm* :set-target
                        :wait-for-opposite-arm)))
+            (:check-grasp-style
+              '(lambda (userdata)
+                 (send *ti* :update-state *arm* :check-grasp-style)
+                 (send *ti* :check-grasp-style *arm*)))
             (:set-target
               '(lambda (userdata)
                  (send *ti* :update-state *arm* :set-target)
diff --git a/jsk_arc2017_baxter/node_scripts/state_server.py b/jsk_arc2017_baxter/node_scripts/state_server.py
index 71739d07c..50323be6b 100755
--- a/jsk_arc2017_baxter/node_scripts/state_server.py
+++ b/jsk_arc2017_baxter/node_scripts/state_server.py
@@ -86,13 +86,14 @@ def _get_can_start(self, state, opposite_state):
         if self.is_pick:
             # pick task: wait condition
             if state == 'wait-for-opposite-arm-start-picking':
-                if opposite_state == 'set-target' \
+                if opposite_state == 'check-grasp-style' \
                         or opposite_state == 'wait-for-user-input':
                     can_start = True
                 else:
                     can_start = False
             elif state == 'wait-for-opposite-arm':
-                if opposite_state == 'set-target' \
+                if opposite_state == 'check-grasp-style' \
+                        or opposite_state == 'set-target' \
                         or opposite_state == 'recognize-object' \
                         or opposite_state == 'set-grasp-style' \
                         or opposite_state == 'pick-object' \

From 15a5095210a4432659950b9365c4b962304556a4 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 22 Jun 2017 15:17:21 +0900
Subject: [PATCH 16/18] Decide pinching direction depending on bbox size

---
 jsk_arc2017_baxter/euslisp/lib/arc-interface.l | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index 27731dac4..54c7979b7 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -215,9 +215,10 @@
   (:pick-object-with-movable-region
     (arm movable-region &key (n-trial 1) (n-trial-same-pos 1)
          (do-stop-grasp nil) (grasp-style :suction))
-    (let (graspingp avs object-index)
+    (let (graspingp avs object-index obj-pos obj-cube)
       ;; TODO: object-index is set randomly
       (setq object-index (random (length (gethash arm object-boxes-))))
+      (setq obj-cube (send self :bbox->cube (elt (gethash arm object-boxes-) object-index)))
       (setq obj-pos
             (send self :get-object-position arm movable-region :object-index object-index))
       (ros::ros-info "[:pick-object-with-movable-region] arm:~a approach to the object" arm)
@@ -253,7 +254,8 @@
               (setq next-av
                     (send *baxter* arm :inverse-kinematics
                           (make-coords :pos (v+ obj-pos #f(0 0 200))
-                                       :rpy (float-vector pi/2 0 0))
+                                       :rpy (if (> (x-of-cube obj-cube) (y-of-cube obj-cube))
+                                              (float-vector pi/2 0 0) (float-vector 0 0 0)))
                           :move-palm-end t
                           :rotation-axis t))))
           (setq gripper-x (send *baxter* arm :gripper-x :joint-angle))
@@ -280,7 +282,7 @@
                   (if (eq grasp-style :suction)
                     (send self :try-to-suction-object arm obj-pos
                           :offset (float-vector 0 0 (- (* i -50) 30)))
-                    (send self :try-to-pinch-object arm obj-pos
+                    (send self :try-to-pinch-object arm obj-pos obj-cube
                           :offset (float-vector 0 0 (- (* i -50) 30))))))))
       (when do-stop-grasp (unless graspingp (send *ri* :stop-grasp arm)))
       (send *ri* :wait-interpolation)
@@ -398,7 +400,7 @@
       (ros::ros-info "[:try-to-suction-object] arm:~a graspingp: ~a" arm graspingp)
     graspingp))
   (:try-to-pinch-object
-    (arm obj-pos &key (offset #f(0 0 0)))
+    (arm obj-pos obj-cube &key (offset #f(0 0 0)))
     (let (graspingp)
       ;; Initialize finger
       (send *ri* :move-hand arm
@@ -409,7 +411,8 @@
       (send *ri* :angle-vector-raw
             (send *baxter* arm :inverse-kinematics
                   (make-coords :pos (v+ obj-pos offset)
-                               :rpy (float-vector pi/2 0 0))
+                               :rpy (if (> (x-of-cube obj-cube) (y-of-cube obj-cube))
+                                      (float-vector pi/2 0 0) (float-vector 0 0 0)))
                   :move-palm-end t
                   :rotation-axis t)
             3000 (send *ri* :get-arm-controller arm) 0)
@@ -419,7 +422,8 @@
       (send *ri* :angle-vector-raw
             (send *baxter* arm :inverse-kinematics
                   (make-coords :pos obj-pos
-                               :rpy (float-vector pi/2 0 0))
+                               :rpy (if (> (x-of-cube obj-cube) (y-of-cube obj-cube))
+                                      (float-vector pi/2 0 0) (float-vector 0 0 0)))
                   :move-palm-end t
                   :rotation-axis t)
             3000 (send *ri* :get-arm-controller arm) 0)

From cc01d74c3f2867a0e7f8a8f69e30b7775cdb56f8 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 22 Jun 2017 15:18:04 +0900
Subject: [PATCH 17/18] :hand -> :pinch

---
 jsk_arc2017_baxter/euslisp/lib/arc-interface.l    | 2 +-
 jsk_arc2017_baxter/euslisp/lib/baxter-interface.l | 6 +++---
 jsk_arc2017_baxter/euslisp/lib/pick-interface.l   | 6 +++---
 jsk_arc2017_baxter/euslisp/lib/stow-interface.l   | 6 +++---
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
index 54c7979b7..591f0baf7 100755
--- a/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/arc-interface.l
@@ -435,7 +435,7 @@
             3000 (send *ri* :get-arm-controller arm) 0)
       (send *ri* :wait-interpolation-until arm :grasp :prismatic-loaded)
       (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
-      (send *ri* :start-grasp arm :hand)
+      (send *ri* :start-grasp arm :pinch)
       (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t))
       (setq graspingp (send *ri* :graspingp arm))
       (ros::ros-info "[:try-to-pinch-object] arm:~a graspingp: ~a" arm graspingp)
diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
index aa5eb4557..fd5732579 100644
--- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l
@@ -222,7 +222,7 @@
                (ros::publish
                  (format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
                  (instance std_msgs::Bool :init :data t))))
-            ((eq type :hand)
+            ((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)
@@ -248,7 +248,7 @@
                (ros::publish
                  (format nil "/vacuum_gripper/limb/~a" (arm2str l/r))
                  (instance std_msgs::Bool :init :data nil))))
-            ((eq type :hand)
+            ((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)
@@ -294,7 +294,7 @@
                        (> (send self :get-finger-load arm) 0.55))))
           (send self :spin-once)
           (or (if (not (eq type :suction)) fingerp nil)
-              (if (not (eq type :hand))
+              (if (not (eq type :pinch))
                 (< (gethash arm pressure-)
                    (cond
                      ((eq arm :rarm) rarm-pressure-threshold-)
diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index 742c4cd46..c40fff711 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -132,7 +132,7 @@
   (:return-from-pick-object (arm)
     (send *ri* :stop-grasp arm :suction)
     (when (eq arm :rarm)
-      (send *ri* :stop-grasp arm :hand)
+      (send *ri* :stop-grasp arm :pinch)
       (send *ri* :move-hand arm
             (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (send *ri* :angle-vector-sequence
@@ -198,7 +198,7 @@
           (ros::ros-error "[main] arm ~a: dropped object" arm)
           (send *ri* :stop-grasp arm :suction)
           (when (eq arm :rarm)
-            (send *ri* :stop-grasp arm :hand)
+            (send *ri* :stop-grasp arm :pinch)
             (send *ri* :move-hand arm
                   (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
         (progn
@@ -211,7 +211,7 @@
           (send *ri* :wait-interpolation)
           (send *ri* :stop-grasp arm :suction) ;; release object
           (if (eq arm :rarm)
-            (send *ri* :stop-grasp arm :hand)) ;; release object
+            (send *ri* :stop-grasp arm :pinch)) ;; release object
           (send self :spin-off-by-wrist arm :times 10)
           (send *ri* :wait-interpolation)
           (send *ri* :angle-vector-raw
diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
index 0e3c47561..46e23d616 100644
--- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
@@ -100,7 +100,7 @@
   (:return-from-pick-object (arm)
     (send *ri* :stop-grasp arm :suction)
     (when (eq arm :rarm)
-      (send *ri* :stop-grasp arm :hand)
+      (send *ri* :stop-grasp arm :pinch)
       (send *ri* :move-hand arm
             (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (ros::ros-info "[main] arm: ~a return from pick-object to fold-pose-back" arm)
@@ -170,7 +170,7 @@
           (ros::ros-error "[main] arm ~a: dropped object" arm)
           (send *ri* :stop-grasp arm :suction)
           (when (eq arm :rarm)
-            (send *ri* :stop-grasp arm :hand)
+            (send *ri* :stop-grasp arm :pinch)
             (send *ri* :move-hand arm
                   (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
         (progn
@@ -183,7 +183,7 @@
           (send *ri* :wait-interpolation)
           (send *ri* :stop-grasp arm :suction) ;; release object
           (if (eq arm :rarm)
-            (send *ri* :stop-grasp arm :hand)) ;; release object
+            (send *ri* :stop-grasp arm :pinch)) ;; release object
           (send self :spin-off-by-wrist arm :times 10)
           (send *ri* :wait-interpolation)
           (send *ri* :angle-vector-raw

From 833f3d0e186aa0898b768a7b513254c13c781eea Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Thu, 22 Jun 2017 15:57:26 +0900
Subject: [PATCH 18/18] Speed up return-from-pick-object

---
 jsk_arc2017_baxter/euslisp/lib/pick-interface.l | 10 +++++-----
 jsk_arc2017_baxter/euslisp/lib/stow-interface.l | 10 +++++-----
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index c40fff711..294508175 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -131,15 +131,15 @@
     graspingp)
   (:return-from-pick-object (arm)
     (send *ri* :stop-grasp arm :suction)
-    (when (eq arm :rarm)
-      (send *ri* :stop-grasp arm :pinch)
-      (send *ri* :move-hand arm
-            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (send *ri* :angle-vector-sequence
           (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f))
                 (send *baxter* :fold-pose-back arm))
           :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
-    (send *ri* :wait-interpolation))
+    (send *ri* :wait-interpolation)
+    (when (eq arm :rarm)
+      (send *ri* :stop-grasp arm :pinch)
+      (send *ri* :move-hand arm
+            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
   (:verify-object (arm)
     (setq picking-fail-count 0)
     ;; TODO: in-hand verification
diff --git a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
index 46e23d616..b0f6d8aa9 100644
--- a/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/stow-interface.l
@@ -99,16 +99,16 @@
     graspingp)
   (:return-from-pick-object (arm)
     (send *ri* :stop-grasp arm :suction)
-    (when (eq arm :rarm)
-      (send *ri* :stop-grasp arm :pinch)
-      (send *ri* :move-hand arm
-            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000))
     (ros::ros-info "[main] arm: ~a return from pick-object to fold-pose-back" arm)
     (send *ri* :angle-vector-sequence
           (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f))
                 (send *baxter* :fold-pose-back arm))
           :fast (send *ri* :get-arm-controller arm) 0 :scale 5.0)
-    (send *ri* :wait-interpolation))
+    (send *ri* :wait-interpolation)
+    (when (eq arm :rarm)
+      (send *ri* :stop-grasp arm :pinch)
+      (send *ri* :move-hand arm
+            (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000)))
   (:verify-object (arm)
     (setq picking-fail-count 0)
     ;; TODO: in-hand verification