Differences

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

Link to this comparison view

Next revision		Previous revision
tutorials:advanced:cram-writing-designators [2022/07/11 13:46] – created vanessatutorials: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:package:cram_designators|cram_designators]]. 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:package:cram_designators|cram_designators]].
  
-===== Syntax ===== +====== Designator ======
  
-<code lisp> +{{:tutorials:advanced:designator-overview-tr.png|}}
-(defun pour (&key +
-               ((:object ?object-designator)) +
-               ((:object-name  ?object-name)) +
-               ((:object-type ?object-type)) +
-               ((:arms ?arms)) +
-               ((:grasp ?grasp)) +
-               ((:left-approach-poses ?left-approach-poses)) +
-               ((:right-approach-poses ?right-approach-poses)) +
-               ((:left-tilt-poses ?left-tilt-poses)) +
-               ((:right-tilt-poses ?right-tilt-poses)) +
-               ((:collision-mode ?collision-mode)) +
-        ((:context ?context)) +
-             &allow-other-keys) +
-  "Object already in hand, approach 2nd object, tilt 100degree, tilt back" +
-   +
-  (roslisp:ros-info (cut-pour pour) "Approaching") +
-  (cpl:with-failure-handling +
-      ((common-fail:manipulation-low-level-failure (e) +
-         (roslisp:ros-warn (cut-and-pour-plans pour) +
-                           "Manipulation messed up: ~a~%Ignoring." +
-                           e) +
-         ;; (return) +
-         )) +
-    (exe:perform +
-     (desig:an action +
-               (type approaching) +
-               (left-poses ?left-approach-poses) +
-               (right-poses ?right-approach-poses) +
-               (desig:when ?collision-mode +
-                 (collision-mode ?collision-mode))))) +
-    (cpl:sleep 2) +
-     +
-    (roslisp:ros-info (cut-pour pour) "Tilting") +
-    (cpl:with-failure-handling +
-        ((common-fail:manipulation-low-level-failure (e) +
-           (roslisp:ros-warn (cut-and-pour-plans pour) +
-                             "Manipulation messed up: ~a~%Ignoring." +
-                             e))) +
-      (exe:perform +
-       (desig:an action +
-                 (type tilting) +
-                 (left-poses ?left-tilt-poses) +
-                 (right-poses ?right-tilt-poses) +
-                 (desig:when ?collision-mode +
-                   (collision-mode ?collision-mode)))))+
  
-  (cpl:sleep 2) 
-  (if (eq ?context :pancake-making) 
-      (and  
-       (roslisp:ros-info (cut-pour pour) "Squeeze") 
-       (exe:perform 
- (desig:an action 
-   (type setting-gripper) 
-   (gripper ?arms) 
-   (position 0.018))) 
  
 +====== Trajectory ======
  
-       (exe:perform +{{:tutorials:advanced:trajectory.png|}}
- (desig:an action +
-   (type setting-gripper) +
-   (gripper ?arms) +
-   (position 0.1))))) +
- +
-  (if (eq ?context :pouring) +
-      (and  +
-       (cpl:with-failure-handling +
-    ((common-fail:manipulation-low-level-failure (e) +
-       (roslisp:ros-warn (cut-and-pour-plans pour) +
- "Manipulation messed up: ~a~%Ignoring." +
- e))) +
- (exe:perform +
-   (desig:an action +
-     (type approaching) +
-     (left-poses ?left-approach-poses) +
-     (right-poses ?right-approach-poses) +
-     (desig:when ?collision-mode +
-       (collision-mode ?collision-mode)))))))) +
-</code> +
-          +
-          +
-<code lisp> +
-(<- (desig:action-grounding ?action-designator (pour ?resolved-action-designator)) +
-    (spec:property ?action-designator (:type :pouring)) +
-    ;; extract info from ?action-designator +
-    (spec:property ?action-designator (:object ?object-designator)) +
-    (desig:current-designator ?object-designator ?current-object-desig) +
-    (spec:property ?current-object-desig (:type ?object-type)) +
-    (spec:property ?current-object-desig (:name ?object-name)) +
-     (-> (spec:property ?action-designator (:arms ?arms)) +
-        (true) +
-        (and (man-int:robot-free-hand ?_ ?arm) +
-             (equal ?arms (?arm)))) +
-     (lisp-fun man-int:get-object-transform ?current-object-desig ?object-transform) +
-      +
-     ;; infer missing information like ?grasp type, gripping ?maximum-effort, manipulation poses +
-     (lisp-fun man-int:calculate-object-faces ?object-transform (?facing-robot-face ?bottom-face)) +
-     (-> (man-int:object-rotationally-symmetric ?object-type) +
- (equal ?rotationally-symmetric t) +
- (equal ?rotationally-symmetric nil)) +
-     (-> (spec:property ?action-designator (:grasp ?grasp)) +
- (true) +
- (and (member ?arm ?arms) +
-       (lisp-fun man-int:get-action-grasps ?object-type ?arm ?object-transform ?grasps) +
-       (member ?grasp ?grasps))) +
-     (lisp-fun man-int:get-action-gripping-effort ?object-type ?effort) +
-     (lisp-fun man-int:get-action-gripper-opening ?object-type ?gripper-opening) +
- +
-     (-> (spec:property ?action-designator (:context ?context)) +
- (true) +
- (format t "WARNING: pouring is only legit with context")) +
-      +
-     ;; calculate trajectory +
-     (equal ?objects (?current-object-desig)) +
-     (-> (member :left ?arms) +
- (and +
-   (-> (equal ?context :pouring) +
-       (lisp-fun man-int:get-action-trajectory :pouring :left ?grasp T +
- ?objects :tilt-angle 100  +
- ?left-pouring-pose) +
-       (lisp-fun man-int:get-action-trajectory :pouring :left ?grasp T +
- ?objects :tilt-angle 160 +
- ?left-pouring-pose)) +
-   (lisp-fun man-int:get-traj-poses-by-label ?left-pouring-pose :approach +
-     ?left-approach-poses) +
-   (lisp-fun man-int:get-traj-poses-by-label ?left-pouring-pose :tilting +
-     ?left-tilt-poses)) +
-              +
-        (and (equal ?left-approach-poses NIL) +
-             (equal ?left-tilt-poses NIL))) +
- +
-     (-> (member :right ?arms) +
- (and +
-   (-> (equal ?context :pouring) +
-       (lisp-fun man-int:get-action-trajectory :pouring :right ?grasp T +
- ?objects :tilt-angle 100 +
- ?right-pouring-pose) +
-       (lisp-fun man-int:get-action-trajectory :pouring :right ?grasp T +
- ?objects :tilt-angle 160 +
-                       ?right-pouring-pose)) +
-             (lisp-fun man-int:get-traj-poses-by-label ?right-pouring-pose :approach +
-                       ?right-approach-poses) +
-             (lisp-fun man-int:get-traj-poses-by-label ?right-pouring-pose :tilting +
-                       ?right-tilt-poses)) +
-              +
-        (and (equal ?right-approach-poses NIL) +
-             (equal ?right-tilt-poses NIL))) +
- +
-     (-> (desig:desig-prop ?action-designator (:collision-mode ?collision-mode)) +
-        (true) +
-        (equal ?collision-mode nil)) +
- +
-     (-> (desig:desig-prop ?action-designator (:context ?context)) +
- (true) +
- (format t "WARNING: pouring action need a context")) +
-    +
- +
-     ;; put together resulting action designator +
-    (desig:designator :action ((:type :pouring) +
-                               (:object ?current-object-desig) +
-                               (:object-type ?object-type) +
-                               (:object-name  ?object-name) +
-                               (:arms ?arms) +
-                               (:grasp ?grasp) +
-                               (:left-approach-poses ?left-approach-poses) +
-                               (:right-approach-poses ?right-approach-poses) +
-                               (:left-tilt-poses ?left-tilt-poses) +
-                               (:right-tilt-poses ?right-tilt-poses) +
-                               (:collision-mode ?collision-mode) +
-        (:context ?context)) +
-                      ?resolved-action-designator)) +
-</code>          +
- +
- +
-<code lisp> +
- +
-;;get pouring trajectory workes like picking-up it will get the  +
-;;object-type-to-gripper-tilt-approch-transform und makes a traj-segment out of it +
-;;here we have only the approach pose, followed by that is the titing pose (above) +
-(defmethod man-int:get-action-trajectory :heuristics 20 ((action-type (eql :pouring)) +
-                                                         arm +
-                                                         grasp +
-                                                         location +
-                                                         objects-acted-on +
-                                                         &key tilt-angle) +
-  (let* ((object +
-           (car objects-acted-on)) +
-         (object-name +
-           (desig:desig-prop-value object :name)) +
-         (object-type +
-           (desig:desig-prop-value object :type)) +
-         (bTo +
-           (man-int:get-object-transform object)) +
-         ;; 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. +
-         (bTb-offset +
-           (man-int::get-object-type-robot-frame-tilt-approach-transform +
-            object-type arm grasp)) +
-         ;; Since the grippers orientation should not depend on the +
-         ;; orientation of the object it is omitted here. +
-         (oTg-std +
-           (cram-tf:copy-transform-stamped +
-            (man-int:get-object-type-to-gripper-transform +
-             object-type object-name arm grasp) +
-            :rotation (cl-tf:make-identity-rotation))) +
-         (approach-pose +
-           (cl-tf:copy-pose-stamped  +
-            (man-int:calculate-gripper-pose-in-base +
-              (cram-tf:apply-transform +
-               (cram-tf:copy-transform-stamped  +
-                bTb-offset +
-                :rotation (cl-tf:make-identity-rotation)) +
-               bTo) +
-              arm oTg-std) +
-            :orientation  +
-            (cl-tf:rotation bTb-offset))) +
-         (tilting-poses +
-           (get-tilting-poses grasp (list approach-pose) (cram-math:degrees->radians tilt-angle)))) +
-    (mapcar (lambda (label poses-in-base) +
-              (man-int:make-traj-segment +
-               :label label +
-               :poses (mapcar  +
-                       (lambda (pose-in-base) +
-                         (let ((mTb (cram-tf:pose->transform-stamped +
-                                     cram-tf:*fixed-frame* +
-                                     cram-tf:*robot-base-frame* +
-                                     0.0 +
-                                     (btr:pose (btr:get-robot-object)))) +
-                               (bTg-std +
-                                 (cram-tf:pose-stamped->transform-stamped +
-                                  pose-in-base +
-                                  (cl-tf:child-frame-id bTo)))) +
-                           (cl-tf:ensure-pose-stamped +
-                            (cram-tf:apply-transform mTb bTg-std)))) +
-                       poses-in-base))) +
-            '(:approach +
-              :tilting) +
-            `((,approach-pose) +
-              ,tilting-poses)))) +
-</code> +
- +
-<code lisp> +
-(defun translate-pose-in-base (bTg &key (x-offset 0.0) (y-offset 0.0) (z-offset 0.0)) +
-  (cram-tf:translate-transform-stamped bTg +
-                                       :x-offset x-offset +
-                                       :y-offset y-offset +
-                                       :z-offset z-offset)) +
- +
-(defun calculate-init-slicing-pose (object arm bTg) +
-  (let* ((x-gripper-position-offset +
-           (/(cl-transforms:y +
-              (cl-bullet::bounding-box-dimensions +
-               (btr:aabb object))) +
-             2)) +
-         (y-gripper-position-offset +
-           (/(cl-transforms:x +
-              (cl-bullet::bounding-box-dimensions +
-               (btr:aabb object))) +
-             2))) +
-    (translate-pose-in-base  +
-     bTg +
-     :x-offset (- x-gripper-position-offset) +
-     :y-offset (if (eq arm :right) +
-                   (- y-gripper-position-offset) +
-                   y-gripper-position-offset)))) +
-</code> +
- +
-<code lisp> +
-(defun get-tilting-poses (grasp approach-poses &optional angle) +
-  (mapcar (lambda (?approach-pose) +
-            ;;depending on the grasp the angle to tilt is different +
-            (case grasp +
-              (:front (rotate-once-pose ?approach-pose (+ angle) :y)) +
-              (:top-front (rotate-once-pose ?approach-pose (+ angle) :y)) +
-              (:left-side (rotate-once-pose ?approach-pose (+ angle) :x)) +
-              (:top-left (rotate-once-pose ?approach-pose (+ angle) :x)) +
-              (:right-side (rotate-once-pose ?approach-pose (- angle) :x)) +
-              (:top-right (rotate-once-pose ?approach-pose (- angle) :x)) +
-              (:back (rotate-once-pose ?approach-pose (- angle) :y)) +
-              (:top (rotate-once-pose ?approach-pose (- angle) :y)) +
-              (t (error "can only pour from :side, back or :front :top :top-side")))) +
-          approach-poses)) +
-</code> +
-<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:copy-pose-stamped +
-   pose +
-   :orientation (let ((pose-orientation (cl-transforms:orientation pose))) +
-                  (cl-tf:normalize +
-                   (cl-transforms:q* +
-                    (cl-transforms:axis-angle->quaternion +
-                     (case axis +
-                       (:x (cl-transforms:make-3d-vector 1 0 0)) +
-                       (:y (cl-transforms:make-3d-vector 0 1 0)) +
-                       (:z (cl-transforms:make-3d-vector 0 0 1)) +
-                       (t (error "in ROTATE-ONCE-POSE forgot to specify axis properly: ~a" axis))) +
-                     angle) +
-                    pose-orientation))))) +
-</code> +
- +
-<code lisp> +
-;;;;;;;;;;;;;; CUP ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +
- +
-(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform  +
-    ((object-type (eql :cup)) +
-     arm +
-     (grasp (eql :left-side))) +
-  '((0.0 0.085 0.065)(0 0 -0.707 0.707))) +
- +
-(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform +
-    ((object-type (eql :cup)) +
-     arm +
-     (grasp (eql :right-side))) +
-  '((0.0 -0.085 0.065)(0 0 0.707 0.707))) +
- +
-(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform +
-    ((object-type (eql :cup)) +
-     arm +
-     (grasp (eql :front))) +
-  '((-0.085 0.0 0.065)(0 0 0 1))) +
- +
-(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform  +
-    ((object-type (eql :cup)) +
-     arm +
-     (grasp (eql :back))) +
-  '((0.085 0.0 0.065)(0 0 1 0))) +
-</code>+