From 8a530b1599d7e7d9d199ce8234be95e366fa6886 Mon Sep 17 00:00:00 2001
From: pazeshun <pazeshun3684@gmail.com>
Date: Sat, 25 Jun 2016 11:47:09 +0900
Subject: [PATCH] Rolling gripper on closer point to robot

---
 .../jsk_2016_01_baxter_apc/baxter-interface.l | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l
index 354bd7438..c027b676f 100644
--- a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l
+++ b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l
@@ -58,6 +58,7 @@
     (sethash :offset-object-h _hard-coded-variables -10)
     (sethash :offset-gripper-bin-top _hard-coded-variables 10)
     (sethash :offset-gripper-bin-side _hard-coded-variables 40)
+    (sethash :draw-out-from-bin-l _hard-coded-variables 80)
 
     (unless (ros::get-param "/apc_on_gazebo" nil)
       (ros::advertise "/vacuum_gripper/limb/left" std_msgs::Bool)
@@ -531,7 +532,8 @@
           (setq offset-from-entrance (float-vector -30 0 0))  ;; when gripper goes straight
           (setq offset-from-entrance
                 (float-vector
-                  -30 0
+                  (if (= sign 0) -30 (- (gethash :draw-out-from-bin-l _hard-coded-variables)))
+                  0
                   (- (/ bin-z-l 2) gripper-req-l
                      (gethash :offset-gripper-bin-top _hard-coded-variables))
                   )
@@ -552,7 +554,7 @@
           (setq bin-dim-x (m->mm (send (send bin-box :dimensions) :x)))
           (send end-coords :translate
                 (float-vector
-                  (- (/ bin-dim-x 2))
+                  (- (+ (/ bin-dim-x 2) (gethash :draw-out-from-bin-l _hard-coded-variables)))
                   (* sign (- gripper-req-l (/ bin-y-l 2)
                              (- (gethash :offset-gripper-bin-side _hard-coded-variables))))
                   0)
@@ -564,6 +566,13 @@
                   end-coords
                   )
             avs-before-approach)
+          (pushback
+            (send *baxter* arm :move-end-pos
+                  (float-vector
+                    (gethash :draw-out-from-bin-l _hard-coded-variables)
+                    0 0)
+                  :world)
+            avs-before-approach)
           (pushback
             (send *baxter* arm :move-end-pos
                   (float-vector
@@ -758,7 +767,11 @@
       (send self :angle-vector-sequence
             (list
               (send *baxter* :angle-vector _last-av-before-approach)
-              (send *baxter* arm :move-end-pos #f(-50 0 0) :world)
+              (send *baxter* arm :move-end-pos
+                    (float-vector
+                      (- (gethash :draw-out-from-bin-l _hard-coded-variables))
+                      0 0)
+                    :world)
               (send self :ik->bin-entrance arm bin
                     :offset (float-vector
                               -100 0 (gethash :offset-avoid-bin-top _hard-coded-variables))