Differences
This shows you the differences between two versions of the page.
Next revision | Previous revision | ||
tutorials:advanced:cram-writing-designators [2022/07/11 13:46] – created vanessa | tutorials:advanced:cram-writing-designators [2022/07/11 15:04] (current) – writing designators update pictures vanessa | ||
---|---|---|---|
Line 4: | Line 4: | ||
Designators are Common Lisp objects for describing various parameters in the [[cram_language|CRAM Plan Language]]. This tutorial only serves the purpose to show how a designator is practically built-in, the theoretical should be taken from this doc [[doc: | Designators are Common Lisp objects for describing various parameters in the [[cram_language|CRAM Plan Language]]. This tutorial only serves the purpose to show how a designator is practically built-in, the theoretical should be taken from this doc [[doc: | ||
- | ===== Syntax | + | ====== Designator ====== |
- | <code lisp> | + | {{:tutorials:advanced:designator-overview-tr.png|}} |
- | (defun pour (&key | + | |
- | ((:object ? | + | |
- | ((:object-name | + | |
- | ((:object-type ?object-type)) | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | & | + | |
- | " | + | |
- | + | ||
- | (roslisp: | + | |
- | (cpl: | + | |
- | ((common-fail: | + | |
- | | + | |
- | " | + | |
- | e) | + | |
- | ;; (return) | + | |
- | )) | + | |
- | (exe: | + | |
- | | + | |
- | (type approaching) | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (cpl:sleep 2) | + | |
- | + | ||
- | (roslisp: | + | |
- | (cpl: | + | |
- | ((common-fail: | + | |
- | | + | |
- | " | + | |
- | | + | |
- | (exe: | + | |
- | | + | |
- | (type tilting) | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | (cpl:sleep 2) | ||
- | (if (eq ?context : | ||
- | (and | ||
- | | ||
- | | ||
- | (desig:an action | ||
- | (type setting-gripper) | ||
- | (gripper ?arms) | ||
- | (position 0.018))) | ||
+ | ====== Trajectory ====== | ||
- | (exe:perform | + | {{:tutorials:advanced: |
- | (desig:an action | + | |
- | (type setting-gripper) | + | |
- | (gripper ?arms) | + | |
- | (position 0.1))))) | + | |
- | + | ||
- | (if (eq ? | + | |
- | (and | + | |
- | | + | |
- | | + | |
- | (roslisp: | + | |
- | " | + | |
- | e))) | + | |
- | (exe: | + | |
- | (desig:an action | + | |
- | (type approaching) | + | |
- | (left-poses ? | + | |
- | (right-poses ? | + | |
- | (desig:when ? | + | |
- | (collision-mode ? | + | |
- | </ | + | |
- | + | ||
- | + | ||
- | <code lisp> | + | |
- | (<- (desig: | + | |
- | (spec: | + | |
- | ;; extract info from ? | + | |
- | (spec: | + | |
- | (desig: | + | |
- | (spec: | + | |
- | (spec: | + | |
- | | + | |
- | (true) | + | |
- | (and (man-int: | + | |
- | | + | |
- | | + | |
- | + | ||
- | ;; infer missing information like ?grasp type, gripping ? | + | |
- | | + | |
- | | + | |
- | (equal ? | + | |
- | (equal ? | + | |
- | | + | |
- | (true) | + | |
- | (and (member ?arm ?arms) | + | |
- | (lisp-fun man-int: | + | |
- | (member ?grasp ? | + | |
- | | + | |
- | | + | |
- | + | ||
- | | + | |
- | (true) | + | |
- | (format t " | + | |
- | + | ||
- | ;; calculate | + | |
- | | + | |
- | | + | |
- | (and | + | |
- | (-> (equal ?context :pouring) | + | |
- | (lisp-fun man-int: | + | |
- | ?objects :tilt-angle 100 | + | |
- | ? | + | |
- | (lisp-fun man-int: | + | |
- | ?objects :tilt-angle 160 | + | |
- | ? | + | |
- | (lisp-fun man-int: | + | |
- | ? | + | |
- | (lisp-fun man-int: | + | |
- | ? | + | |
- | + | ||
- | (and (equal ? | + | |
- | | + | |
- | + | ||
- | | + | |
- | (and | + | |
- | (-> (equal ?context :pouring) | + | |
- | (lisp-fun man-int: | + | |
- | ?objects :tilt-angle 100 | + | |
- | ? | + | |
- | (lisp-fun man-int: | + | |
- | ?objects :tilt-angle 160 | + | |
- | ? | + | |
- | | + | |
- | ? | + | |
- | | + | |
- | ? | + | |
- | + | ||
- | (and (equal ? | + | |
- | | + | |
- | + | ||
- | | + | |
- | (true) | + | |
- | (equal ? | + | |
- | + | ||
- | | + | |
- | (true) | + | |
- | (format t " | + | |
- | + | ||
- | + | ||
- | ;; put together resulting action designator | + | |
- | (desig: | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | ? | + | |
- | </ | + | |
- | + | ||
- | + | ||
- | <code lisp> | + | |
- | + | ||
- | ;;get pouring trajectory workes like picking-up it will get the | + | |
- | ;; | + | |
- | ;;here we have only the approach pose, followed by that is the titing pose (above) | + | |
- | (defmethod man-int: | + | |
- | arm | + | |
- | | + | |
- | | + | |
- | | + | |
- | & | + | |
- | (let* ((object | + | |
- | (car objects-acted-on)) | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | | + | |
- | ;; The first part of the btb-offset transform encodes the | + | |
- | ;; translation difference between the gripper and the | + | |
- | ;; object. The static defined orientation of bTb-offset | + | |
- | ;; describes how the gripper should be orientated to approach | + | |
- | ;; the object in which something should be poured into. This | + | |
- | ;; depends mostly on the defined coordinate frame of the | + | |
- | ;; object and how objects should be rotated to pour something | + | |
- | ;; out of them. | + | |
- | | + | |
- | | + | |
- | object-type arm grasp)) | + | |
- | ;; Since the grippers orientation should not depend on the | + | |
- | ;; orientation of the object it is omitted here. | + | |
- | | + | |
- | | + | |
- | (man-int: | + | |
- | | + | |
- | :rotation (cl-tf: | + | |
- | | + | |
- | | + | |
- | (man-int: | + | |
- | (cram-tf: | + | |
- | | + | |
- | bTb-offset | + | |
- | :rotation (cl-tf: | + | |
- | | + | |
- | arm oTg-std) | + | |
- | : | + | |
- | (cl-tf: | + | |
- | | + | |
- | | + | |
- | (mapcar (lambda (label poses-in-base) | + | |
- | (man-int: | + | |
- | : | + | |
- | : | + | |
- | | + | |
- | (let ((mTb (cram-tf: | + | |
- | | + | |
- | | + | |
- | 0.0 | + | |
- | | + | |
- | | + | |
- | | + | |
- | pose-in-base | + | |
- | (cl-tf: | + | |
- | | + | |
- | (cram-tf: | + | |
- | | + | |
- | ' | + | |
- | :tilting) | + | |
- | `((, | + | |
- | , | + | |
- | </ | + | |
- | + | ||
- | <code lisp> | + | |
- | (defun translate-pose-in-base (bTg &key (x-offset 0.0) (y-offset 0.0) (z-offset 0.0)) | + | |
- | (cram-tf: | + | |
- | : | + | |
- | : | + | |
- | : | + | |
- | + | ||
- | (defun calculate-init-slicing-pose (object arm bTg) | + | |
- | (let* ((x-gripper-position-offset | + | |
- | | + | |
- | (cl-bullet:: | + | |
- | | + | |
- | 2)) | + | |
- | | + | |
- | | + | |
- | (cl-bullet:: | + | |
- | | + | |
- | | + | |
- | (translate-pose-in-base | + | |
- | bTg | + | |
- | : | + | |
- | : | + | |
- | (- y-gripper-position-offset) | + | |
- | | + | |
- | </ | + | |
- | + | ||
- | <code lisp> | + | |
- | (defun get-tilting-poses (grasp approach-poses & | + | |
- | (mapcar (lambda (? | + | |
- | ;;depending on the grasp the angle to tilt is different | + | |
- | (case grasp | + | |
- | (:front (rotate-once-pose ? | + | |
- | (:top-front (rotate-once-pose ? | + | |
- | (:left-side (rotate-once-pose ? | + | |
- | (:top-left (rotate-once-pose ? | + | |
- | (: | + | |
- | (:top-right (rotate-once-pose ? | + | |
- | (:back (rotate-once-pose ? | + | |
- | (:top (rotate-once-pose ? | + | |
- | (t (error "can only pour from :side, back or :front :top : | + | |
- | approach-poses)) | + | |
- | </ | + | |
- | <code lisp> | + | |
- | ;;helper function for tilting | + | |
- | ;;rotate the pose around the axis in an angle | + | |
- | (defun rotate-once-pose (pose angle axis) | + | |
- | (cl-transforms-stamped: | + | |
- | | + | |
- | : | + | |
- | (cl-tf: | + | |
- | | + | |
- | (cl-transforms: | + | |
- | (case axis | + | |
- | (:x (cl-transforms: | + | |
- | (:y (cl-transforms: | + | |
- | (:z (cl-transforms: | + | |
- | (t (error "in ROTATE-ONCE-POSE forgot to specify axis properly: ~a" axis))) | + | |
- | | + | |
- | pose-orientation))))) | + | |
- | </ | + | |
- | + | ||
- | <code lisp> | + | |
- | ;;;;;;;;;;;;;; | + | |
- | + | ||
- | (defmethod man-int: | + | |
- | ((object-type (eql :cup)) | + | |
- | arm | + | |
- | | + | |
- | '((0.0 0.085 0.065)(0 0 -0.707 0.707))) | + | |
- | + | ||
- | (defmethod man-int: | + | |
- | ((object-type (eql :cup)) | + | |
- | arm | + | |
- | | + | |
- | '((0.0 -0.085 0.065)(0 0 0.707 0.707))) | + | |
- | + | ||
- | (defmethod man-int: | + | |
- | ((object-type (eql :cup)) | + | |
- | arm | + | |
- | | + | |
- | ' | + | |
- | + | ||
- | (defmethod man-int: | + | |
- | ((object-type (eql :cup)) | + | |
- | arm | + | |
- | | + | |
- | ' | + | |
- | </ | + |