From 8a4e5572ca998596dfbef61a7bcc68ae3545b6fe Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 1 May 2020 15:25:29 +0900
Subject: [PATCH] Move robot-common part of simulated grasping to
 robot-interface

---
 pr2eus/pr2-interface.l   | 23 ++++-------------------
 pr2eus/robot-interface.l | 23 +++++++++++++++++++++++
 2 files changed, 27 insertions(+), 19 deletions(-)

diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l
index d6ccdd88d..38abe5858 100644
--- a/pr2eus/pr2-interface.l
+++ b/pr2eus/pr2-interface.l
@@ -266,22 +266,8 @@ Example: (send self :gripper :rarm :position) => 0.00"
    (&optional (arm :arms) &key ((:gain g) 0.01) ((:objects objs) objects) force-assoc ((:wait wait) t))
    (send self :move-gripper arm 0.0 :effort (* 2000 g) :wait wait)
    (unless joint-action-enable
-     (dolist (a (if (eq arm :arms) '(:larm :rarm) (list arm)))
-       (when objs
-         (let ((grasp-convex
-	      (convex-hull-3d
-	       (flatten
-		(mapcar
-		 #'(lambda (l)
-		     (send-all (send l :bodies) :worldcoords)
-		     (send-all (send l :bodies) :vertices))
-		 (send robot a :gripper :links))))))
-	 (dolist (obj objs)
-	   (when (or force-assoc
-               (and (find-method obj :faces)
-                    (not (= (pqp-collision-check grasp-convex obj) 0))))
-	     (if (send obj :parent) (send (send obj :parent) :dissoc obj))
-	     (send robot a :end-coords :assoc obj))))))
+     (send self :assoc-objects-in-gripper
+           (if (eq arm :arms) '(:larm :rarm) (list arm)) :objects objs :force-assoc force-assoc)
      ;; (send self :update-robot-state) ;; update state of 'robot' for real robot
      (return-from :start-grasp
        (case arm
@@ -354,9 +340,8 @@ Example: (send self :gripper :rarm :position) => 0.00"
    (prog1
        (send self :move-gripper arm 0.09 :effort (* 2000 g) :wait wait)
      (unless joint-action-enable
-       (dolist (a (if (eq arm :arms) '(:larm :rarm) (list arm)))
-         (dolist (obj (send robot a :end-coords :descendants))
-           (send robot arm :end-coords :dissoc obj))))))
+       (send self :dissoc-objects-in-gripper
+             (if (eq arm :arms) '(:larm :rarm) (list arm))))))
   ;;
   (:pr2-gripper-state-callback
    (arm msg)
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
index f560ef691..1d8aceef3 100644
--- a/pr2eus/robot-interface.l
+++ b/pr2eus/robot-interface.l
@@ -1053,6 +1053,29 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
        (if spin-self (send self :spin-once))
        (ros::sleep)))
    t)
+  (:assoc-objects-in-gripper
+    (arm-list &key ((:objects objs) objects) force-assoc)
+    (dolist (a arm-list)
+      (when objs
+        (let ((grasp-convex
+                (convex-hull-3d
+                  (flatten
+                    (mapcar
+                      #'(lambda (l)
+                          (send-all (send l :bodies) :worldcoords)
+                          (send-all (send l :bodies) :vertices))
+                      (send robot a :gripper :links))))))
+          (dolist (obj objs)
+            (when (or force-assoc
+                      (and (find-method obj :faces)
+                           (not (= (pqp-collision-check grasp-convex obj) 0))))
+              (if (send obj :parent) (send (send obj :parent) :dissoc obj))
+              (send robot a :end-coords :assoc obj)))))))
+  (:dissoc-objects-in-gripper
+    (arm-list)
+    (dolist (a arm-list)
+      (dolist (obj (send robot a :end-coords :descendants))
+        (send robot a :end-coords :dissoc obj))))
   ;;
   (:go-pos
    (x y &optional (d 0)) ;; [m] [m] [degree]