no way to compare when less than two revisions

Differences

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

@@ Line 1: / Line 1: @@
+====== Solutions to Exercises from Zero prerequisites demo tutorial: Simple fetch and place ======
+==== Exercise 1 ====
+<code lisp>
+;; Simple solution involves adding some hardcoded base poses to search the bottle
+;; Then nesting the find object with another failure handling that iterates different hardcoded
+;; base poses to find the object.
+;; Solution to exercise 1 with static poses
+(defparameter *base-pose-near-table-towards-island*
+  (make-pose "map" '((-1.447d0 0.150d0 0.0d0) (0.0d0 0.0d0 0.7071d0 0.7071d0))))
+(defparameter *base-pose-near-sink-surface*
+  (make-pose "map" '((0.700000 0.650000 0.00000) (0.00000 0.00000 0 1))))
+(defun search-and-pick-up-object (?obj-type)
+  (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)
+        ((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)))))
+        (cpl:retry))
+      (print "Exhausted all the locations to search. Cannot find the object")
+      (cpl:fail 'object-unreachable))))
+(defun move-bottle (bottle-spawn-pose)
+  (spawn-object bottle-spawn-pose)
+  (with-simulated-robot
+    (let ((?navigation-goal *base-pose-near-table*))
+      (cpl:par
+        ;; Moving the robot near the table.
+        (perform (an action
+                     (type going)
+                     (target (a location
+                                (pose ?navigation-goal)))))
+        (perform (a motion
+                    (type moving-torso)
+                    (joint-angle 0.3)))
+        (park-arms)))
+    (let* ((?perceived-bottle-and-grasping-arm (search-and-pick-up-object :bottle))
+           (?grasping-arm (second ?perceived-bottle-and-grasping-arm))
+           (?perceived-bottle (first ?perceived-bottle-and-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>
+==== Exercise 2 ====
+<code lisp>
+;; Solution for choosing the arm based on the position of the robot
+(defun get-preferred-arm (?perceived-object)
+  (let* ((obj-name (get-obj-name ?perceived-object))
+         (obj-pose (get-current-pose-of-object obj-name))
+         (robot-transformation (get-robot-transformation))
+         (inverse-robot-transform (inverse-transformation robot-transformation))
+         ;; Position of the object relative to the robot
+         (obj-in-robot-frame (apply-transformation inverse-robot-transform obj-pose))
+         (x (get-x-of-pose obj-in-robot-frame))
+         (y (get-y-of-pose obj-in-robot-frame)))
+         ;; If the object is in front of the robot
+    (if (> x 0)
+        ;; if the object is on positive y-axis in front of the robot
+        ;; then left, else right
+        (if (> y 0)
+            :left
+            :right)
+        ;; If the object is on positive y-axis while behind the robot
+        ;; then right, else left
+        (if (> y 0)
+            :right
+            :left))))
+</code>
+==== Exercise 3 ====
+<code lisp>
+;; Modified find-object which changes torso link during perception failure
+(defun find-object (?object-type)
+  (let* ((possible-look-directions `(,*downward-look-coordinate*
+                                     ,*left-downward-look-coordinate*
+                                     ,*right-downward-look-coordinate*))
+         (?looking-direction (first possible-look-directions))
+         (possible-torso-link-positions '(0.1 0.2))
+         (?current-torso-link-position 0.3))
+    (setf possible-look-directions (rest possible-look-directions))
+    ;; Look towards the first direction
+    (perform (an action
+                 (type looking)
+                 (target (a location
+                            (pose ?looking-direction)))))
+    ;; Set the first torso link angle
+    (perform (a motion
+                (type moving-torso)
+                (joint-angle ?current-torso-link-position)))
+    ;; perception-object-not-found is the error that we get when the robot cannot find the object.
+    ;; Now we're wrapping it in a failure handling clause to handle it
+    (handle-failure object-nowhere-to-be-found
+        ((handle-failure perception-object-not-found
+             ;; Try the action
+             ((perform (an action
+                           (type detecting)
+                           (object (an object
+                                       (type ?object-type))))))
+           ;; If the action fails, try the following:
+           ;; try different look directions until there is none left.
+           (when possible-look-directions
+             (print "Perception error happened! Turning head.")
+             ;; Resetting the head to look forward before turning again
+             (perform (an action
+                          (type looking)
+                          (direction forward)))
+             (setf ?looking-direction (first possible-look-directions))
+             (setf possible-look-directions (rest possible-look-directions))
+             (perform (a motion
+                         (type looking)
+                         (target (a location
+                                    (pose ?looking-direction)))))
+             ;; This statement retries the action again
+             (cpl:retry))
+           ;; If everything else fails, error out
+           ;; Reset the neck before erroring out
+           (perform (an action
+                        (type looking)
+                        (direction forward)))
+           (cpl:fail 'object-nowhere-to-be-found)))
+      (when possible-torso-link-positions
+        (print "Perception error happened! Changing torso link position")
+        (setf ?current-torso-link-position (first possible-torso-link-positions))
+        (setf possible-torso-link-positions (rest possible-torso-link-positions))
+        (setf possible-look-directions `(,*downward-look-coordinate*
+                                         ,*left-downward-look-coordinate*
+                                         ,*right-downward-look-coordinate*))
+        (setf ?looking-direction (first possible-look-directions))
+        (perform (a motion
+                    (type moving-torso)
+                    (joint-angle ?current-torso-link-position)))
+        (cpl:retry))
+      (print "All Torso link positions exhausted :(")
+      (cpl:fail 'object-nowhere-to-be-found))))
+</code>