====== Solutions to Exercises from Zero prerequisites demo tutorial: Simple fetch and place ======
==== Exercise 1 ====
;; 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 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
(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))))
==== Exercise 2 ====
;; 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))))
==== Exercise 3 ====
;; 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))))