Adding new designators

Disclaimer: Designators are Common Lisp objects for describing various parameters in the 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 cram_designators.

(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)))
 
 
       (exe:perform
	(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))))))))

(<- (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))

;;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))))

(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))))

(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))

;;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)))))

;;;;;;;;;;;;;; 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)))