Differences

This shows you the differences between two versions of the page.

--- tutorials:demo:fetch_and_place_solutions [2019/09/04 19:38] – [Exercise 3] gkazhoya
+++ tutorials:demo:fetch_and_place_solutions [2019/09/04 19:54] (current) – [Exercise 1] gkazhoya
@@ Line 16: / Line 16: @@
   (make-pose "map" '((0.700000 0.650000 0.00000) (0.00000 0.00000 0 1))))
-(defun search-and-pick-up-object (?obj-type)
+(defun move-bottle (bottle-spawn-pose)
-  (let ((?possible-base-poses `(,*base-pose-near-table-towards-island* ,*base-pose-near-sink-surface*))
+  (spawn-object bottle-spawn-pose)
-        (?current-base-pose *base-pose-near-table*))
+  (with-simulated-robot
-    (handle-failure (or object-nowhere-to-be-found
+    (let ((?navigation-goal *base-pose-near-table*))
-                        object-unreachable)
+      (cpl:par
+        ;; Moving the robot near the table.
-        ((let* ((?perceived-bottle (find-object ?obj-type))
-                (?grasping-arm :left)) ; or (get-preferred-arm ?perceived-bottle)
-           ;; We update the value of ?grasping-arm according to what the method used
-           (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm))
-           (park-arm ?grasping-arm)
-           `(,?perceived-bottle ,?grasping-arm)))
-      (when (first ?possible-base-poses)
-        (print "Changing the base to a new location to try finding the object")
-        (setf ?current-base-pose (first ?possible-base-poses))
-        (setf ?possible-base-poses (rest ?possible-base-poses))
         (perform (an action
                      (type going)
                      (target (a location
-                                (pose ?current-base-pose)))))
+                                (pose ?navigation-goal)))))
-        (cpl:retry))
+        (perform (a motion
-      (print "Exhausted all the locations to search. Cannot find the object")
+                    (type moving-torso)
-      (cpl:fail 'object-unreachable))))
+                    (joint-angle 0.3)))
+        (park-arms)))
+    (let ((?perceived-bottle
+            (let ((?possible-base-poses `(,*base-pose-near-table-towards-island*
+                                          ,*base-pose-near-sink-surface*))
+                  (?current-base-pose *base-pose-near-table*))
+              (handle-failure (or object-nowhere-to-be-found
+                                  object-unreachable)
+                  ((find-object :bottle))
+                (when (first ?possible-base-poses)
+                  (print "Changing the base to a new location to try finding the object")
+                  (setf ?current-base-pose (first ?possible-base-poses))
+                  (setf ?possible-base-poses (rest ?possible-base-poses))
+                  (perform (an action
+                               (type going)
+                               (target (a location
+                                          (pose ?current-base-pose)))))
+                  (cpl:retry))
+                (print "Exhausted all the locations to search. Cannot find the object")
+                (cpl:fail 'object-unreachable))))
+          (?grasping-arm :right))
+      ;; We update the value of ?grasping-arm according to what the method used
+      (setf ?grasping-arm (pick-up-object ?perceived-bottle ?grasping-arm))
+      (park-arm ?grasping-arm)
+      ;; Moving the robot near the counter.
+      (let ((?nav-goal *base-pose-near-counter*))
+        (perform (an action
+                     (type going)
+                     (target (a location
+                                (pose ?nav-goal))))))
+       ;; Setting the object down on the counter
+      (let ((?drop-pose *final-object-destination*))
+        (perform (an action
+                     (type placing)
+                     (arm ?grasping-arm)
+                     (object ?perceived-bottle)
+                     (target (a location
+                                (pose ?drop-pose))))))
+      (park-arm ?grasping-arm))))
 </code>