no way to compare when less than two revisions
Differences
This shows you the differences between two versions of the page.
Previous revisionNext revision | |||
— | tutorials:demo:fetch_and_place_solutions [2019/09/04 19:38] – [Exercise 3] gkazhoya | ||
---|---|---|---|
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 " | ||
+ | |||
+ | (defparameter *base-pose-near-sink-surface* | ||
+ | (make-pose " | ||
+ | |||
+ | (defun search-and-pick-up-object (?obj-type) | ||
+ | (let ((? | ||
+ | (? | ||
+ | (handle-failure (or object-nowhere-to-be-found | ||
+ | object-unreachable) | ||
+ | | ||
+ | ((let* ((? | ||
+ | (? | ||
+ | ;; We update the value of ? | ||
+ | (setf ? | ||
+ | | ||
+ | | ||
+ | | ||
+ | (when (first ? | ||
+ | (print " | ||
+ | (setf ? | ||
+ | (setf ? | ||
+ | (perform (an action | ||
+ | (type going) | ||
+ | | ||
+ | (pose ? | ||
+ | (cpl: | ||
+ | (print " | ||
+ | (cpl:fail ' | ||
+ | </ | ||
+ | |||
+ | ==== Exercise 2 ==== | ||
+ | |||
+ | <code lisp> | ||
+ | ;; Solution for choosing the arm based on the position of the robot | ||
+ | (defun get-preferred-arm (? | ||
+ | (let* ((obj-name (get-obj-name ? | ||
+ | | ||
+ | | ||
+ | | ||
+ | ;; Position of the object relative to the robot | ||
+ | | ||
+ | (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 ==== | ||
+ | |||
+ | <code lisp> | ||
+ | |||
+ | ;; Modified find-object which changes torso link during perception failure | ||
+ | |||
+ | (defun find-object (? | ||
+ | (let* ((possible-look-directions `(, | ||
+ | , | ||
+ | , | ||
+ | | ||
+ | | ||
+ | | ||
+ | (setf possible-look-directions (rest possible-look-directions)) | ||
+ | ;; Look towards the first direction | ||
+ | (perform (an action | ||
+ | (type looking) | ||
+ | | ||
+ | (pose ? | ||
+ | ;; Set the first torso link angle | ||
+ | (perform (a motion | ||
+ | (type moving-torso) | ||
+ | (joint-angle ? | ||
+ | ;; 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 | ||
+ | | ||
+ | (type detecting) | ||
+ | | ||
+ | (type ? | ||
+ | |||
+ | ;; If the action fails, try the following: | ||
+ | ;; try different look directions until there is none left. | ||
+ | (when possible-look-directions | ||
+ | | ||
+ | ;; Resetting the head to look forward before turning again | ||
+ | | ||
+ | (type looking) | ||
+ | (direction forward))) | ||
+ | (setf ? | ||
+ | (setf possible-look-directions (rest possible-look-directions)) | ||
+ | | ||
+ | (type looking) | ||
+ | | ||
+ | (pose ? | ||
+ | ;; This statement retries the action again | ||
+ | | ||
+ | ;; If everything else fails, error out | ||
+ | ;; Reset the neck before erroring out | ||
+ | | ||
+ | (type looking) | ||
+ | (direction forward))) | ||
+ | | ||
+ | (when possible-torso-link-positions | ||
+ | (print " | ||
+ | (setf ? | ||
+ | (setf possible-torso-link-positions (rest possible-torso-link-positions)) | ||
+ | (setf possible-look-directions `(, | ||
+ | , | ||
+ | , | ||
+ | (setf ? | ||
+ | (perform (a motion | ||
+ | (type moving-torso) | ||
+ | (joint-angle ? | ||
+ | (cpl: | ||
+ | (print "All Torso link positions exhausted :(") | ||
+ | (cpl:fail ' | ||
+ | |||
+ | |||
+ | </ |