Add set-grasp-style state in pick

start-jsk · Jun 21, 2017 · 353cc07 · 353cc07
1 parent 9b17359
commit 353cc07
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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' \