====== Implementing new types of actions ======

When implementing new type of actions, such as placing, pouring or cutting there are 4 important steps to do.
In the following Tutorial it is assumed that the basic knowledge about [[http://cram-system.org/doc/package/cram_designators|cram-designator]], [[http://cram-system.org/tutorials/beginner/high_level_plans|high_level_plans]] and [[http://cram-system.org/tutorials/beginner/cram_prolog|cram_prolog_reasoning.]] are given.
====== Atomic-action-designator ======

Action designators are symbolic action descriptions that can be converted to actual ROS actions goals. 
How atomic-action-desginators are implemented for picking and placing:

<code lisp>
;;cram/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designator.lisp

(in-package :pp-plans)

(def-fact-group pick-and-place-atomic-actions (desig:action-grounding)

  ;;type going
 (<- (desig:action-grounding ?action-designator (go-to-target ?pose-stamped))
    (spec:property ?action-designator (:type :going))
    (spec:property ?action-designator (:target ?location-designator))
    (desig:designator-groundings ?location-designator ?poses)
    (member ?pose-stamped ?poses))
 
  ;;type grapsing
 (<- (desig:action-grounding ?action-designator (move-arms-in-sequence
                                                  ?left-poses ?right-poses
                                                  :allow-hand
                                                  ?object-name ?object-link))
    (or (spec:property ?action-designator (:type :grasping))
        (spec:property ?action-designator (:type :pulling)))
    (spec:property ?action-designator (:object ?object-designator))
    (spec:property ?object-designator (:name ?object-name))
    (or (spec:property ?action-designator (:link ?object-link))
        (equal ?object-link nil))
    (once (or (spec:property ?action-designator (:left-poses ?left-poses))
              (equal ?left-poses nil)))
    (once (or (spec:property ?action-designator (:right-poses ?right-poses))
              (equal ?right-poses nil))))
              [...]

</code>

Here are all the specs for a designator described. 
First the designator for :going needs a ?pose-stamped as a target to go. Then its specified that the action-designator is of type going and the target will be a ?location-designator. Please remind that variables with '?' in the front are prolog-variables. 


The second designator :grapsing needs more variable like move-arm-in-sequence, ?left-poses, ?right-poses, :allow-hand (here you can decide between different types e.g. :allow-hands, avoid-all & allow-all), ?object-name and ?object-link.
It is important to know the link of the object such that the coordinate frames in the world can be translated from gripper to object so that the robot can pick the object. 


This designator can now either be used for :grapsing or :pulling.

----


When a new type of action is now implemented it is significant to know what specs you will need e.g.

For type :closing-gripper/open-gripper it is important to have specified which hand (?left-or-right-or-both) and the actiontype (?action-type). The Variables will now be bounded to the properties. 

<code lisp>
 (<- (desig:action-grounding ?action-designator (open-or-close-gripper ?left-or-right-or-both
                                                                        ?action-type))
    (or (spec:property ?action-designator (:type :closing-gripper))
        (spec:property ?action-designator (:type :opening-gripper)))
    (spec:property ?action-designator (:type ?action-type))
    (spec:property ?action-designator (:gripper ?left-or-right-or-both)))
    
</code>
====== Pick-action-plan ====== 

After writing the atomic-action-designators the action needs to be implemented as an plan.
What is required for picking up:


  * First the gripper needs to be open to grab an object.
  * Second the hand (gripper) needs to reach the object. 
  * Third the object needs to be hold in the gripper (grasping and gripping).
  * The last thing to do is to lift the object with the gripper. For every single action here a action-designator needs to be called e.g.

<code lisp>
 (exe:perform
     (desig:an action
               (type setting-gripper)
               (gripper ?arm)
               (position ?gripper-opening)))
</code>

The variables that are handed over here to the action-designator needs be called in the parameter-list.

<code lisp>
;;cram/cram_common/cram_mobile_pick_place_plans/src/pick-place-plans.lisp

(in-package :pp-plans)

(cpl:def-cram-function pick-up (?object-designator
                                ?arm ?gripper-opening ?grip-effort ?grasp
                                ?left-reach-poses ?right-reach-poses
                                ?left-grasping-poses ?right-grasping-poses
                                ?left-lift-poses ?right-lift-poses)
  (cram-tf:visualize-marker (man-int:get-object-pose ?object-designator)
                            :r-g-b-list '(1 1 0) :id 300)

  (cpl:par
    (roslisp:ros-info (pick-place pick-up) "Opening gripper")
    (exe:perform
     (desig:an action
               (type setting-gripper)
               (gripper ?arm)
               (position ?gripper-opening)))
    (roslisp:ros-info (pick-place pick-up) "Reaching")
    (cpl:with-failure-handling
        ((common-fail:manipulation-low-level-failure (e)
           (roslisp:ros-warn (pp-plans pick-up)
                             "Manipulation messed up: ~a~%Ignoring."
                             e)
           ;; (return)
           ))
      (exe:perform
       (desig:an action
                 (type reaching)
                 (left-poses ?left-reach-poses)
                 (right-poses ?right-reach-poses)))))
  (cpl:with-failure-handling
      ((common-fail:manipulation-low-level-failure (e)
         (roslisp:ros-warn (pp-plans pick-up)
                           "Manipulation messed up: ~a~%Ignoring."
                           e)
         (return)
         ))
    (exe:perform
     (desig:an action
               (type grasping)
               (object ?object-designator)
               (left-poses ?left-grasping-poses)
               (right-poses ?right-grasping-poses))))
  (roslisp:ros-info (pick-place pick-up) "Gripping")
  (exe:perform
   (desig:an action
             (type gripping)
             (gripper ?arm)
             (effort ?grip-effort)
             (object ?object-designator)))
  (roslisp:ros-info (pick-place pick-up) "Assert grasp into knowledge base")
  (cram-occasions-events:on-event
   (make-instance 'cpoe:object-attached-robot
     :object-name (desig:desig-prop-value ?object-designator :name)
     :arm ?arm
     :grasp ?grasp))
  (roslisp:ros-info (pick-place pick-up) "Lifting")
  (cpl:with-failure-handling
      ((common-fail:manipulation-low-level-failure (e)
         (roslisp:ros-warn (pp-plans pick-up)
                           "Manipulation messed up: ~a~%Ignoring."
                           e)
         (return)))
    (exe:perform
     (desig:an action
               (type lifting)
               (left-poses ?left-lift-poses)
               (right-poses ?right-lift-poses)))))
               [...]
 </code>

Here also failure-handling takes place. At least all low-level-failures should be caught here.

====== Pick-action-designator ======



Now it is necessary to define a action designator of the new type for pick-up e.g.

<code lisp>
;;cram/cram_common/cram_mobile_pick_place_plans/src/pick-place-designators.lisp
(def-fact-group pick-and-place-plans (desig:action-grounding)
  (<- (desig:action-grounding ?action-designator (pick-up ?current-object-desig ?arm
                                                          ?gripper-opening ?effort ?grasp
                                                          ?left-reach-poses ?right-reach-poses
                                                          ?left-grasp-poses ?right-grasp-poses
                                                          ?left-lift-poses ?right-lift-poses))
</code>

First extract all information from ?action-designator.
    
<code lisp>
    (spec:property ?action-designator (:type :picking-up))
    (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 (:arm ?arm))
        (true)
        (man-int:robot-free-hand ?_ ?arm))
    (lisp-fun man-int:get-object-transform ?current-object-desig ?object-transform)
    
    </code>
    
Infer missing information like ?grasp type, gripping ?maximum-effort, manipulation poses. Then calculate the object facing, because the system needs to know how the object, that will be picked up, is facing so that the rotation is correctly and the transformation from gripper to object will be properly.
 
   <code lisp>
 
    (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 (lisp-fun man-int:get-object-type-grasps ?object-type ?arm ?object-transform ?grasps)
             (member ?grasp ?grasps)))
    (lisp-fun man-int:get-object-type-gripping-effort ?object-type ?effort)
    (lisp-fun man-int:get-object-type-gripper-opening ?object-type ?gripper-opening)
    (lisp-fun man-int:get-object-grasping-poses
              ?object-name ?object-type :left ?grasp ?object-transform
              ?left-poses)
    (lisp-fun man-int:get-object-grasping-poses
              ?object-name ?object-type :right ?grasp ?object-transform
              ?right-poses)
    (lisp-fun extract-pick-up-manipulation-poses ?arm ?left-poses ?right-poses
              (?left-reach-poses ?right-reach-poses
                                 ?left-grasp-poses ?right-grasp-poses
                                 ?left-lift-poses ?right-lift-poses)))

  
</code>



    
====== Get-trajectory ======

<code lisp>
;;cram_common/cram_manipulation_interfaces/src/trajectories.lis
(defgeneric get-object-grasping-poses (object-name object-type arm grasp object-transform)
  (:documentation "Returns a list of (pregrasp-pose 2nd-pregrasp-pose grasp-pose lift-pose)")
  (:method (object-name object-type arm grasp object-transform)
    (declare (type symbol object-name object-type arm grasp)
             (type cl-transforms-stamped:transform-stamped object-transform))

    (when (prolog `(object-rotationally-symmetric ,object-type))
      (setf object-transform
            (cram-tf:copy-transform-stamped
             object-transform
             :rotation (cl-transforms:make-identity-rotation))))
</code>
First correct the object transform such that rotationally-symmetric objects would not be grasped in an awkward way with weird orientations. 

<code lisp>
    (let* ((gripper-tool-frame
             (ecase arm
               (:left cram-tf:*robot-left-tool-frame*)
               (:right cram-tf:*robot-right-tool-frame*)))
           (object-to-standard-gripper-transform ; oTg'
             (get-object-type-to-gripper-transform object-type object-name arm grasp))
           (object-to-standard-gripper-pregrasp-transform ; oTg'
             (get-object-type-to-gripper-pregrasp-transform
              object-type object-name arm grasp
              object-to-standard-gripper-transform))
           (object-to-standard-gripper-2nd-pregrasp-transform ; oTg'
             (get-object-type-to-gripper-2nd-pregrasp-transform
              object-type object-name arm grasp
              object-to-standard-gripper-transform))
           (object-to-standard-gripper-lift-transform ; oTg'
             (get-object-type-to-gripper-lift-transform
              object-type object-name arm grasp
              object-to-standard-gripper-transform))
           (object-to-standard-gripper-2nd-lift-transform ; oTg'
             (get-object-type-to-gripper-2nd-lift-transform
              object-type object-name arm grasp
              object-to-standard-gripper-transform))
           (standard-to-particular-gripper-transform ; g'Tg
             (cl-transforms-stamped:transform->transform-stamped
              gripper-tool-frame
              gripper-tool-frame
              0.0
              (cut:var-value
               '?transform
               (car (prolog:prolog
                     `(and (cram-robot-interfaces:robot ?robot)
                           (cram-robot-interfaces:standard-to-particular-gripper-transform
                            ?robot ?transform))))))))
                            
 </code>
Next step is to transform all the poses for the action.
  * standard gripper transform oTg 
  * standard gripper pregrasp transform oTg
  * standard gripper second pregrasp transform oTg
  * standard gripper lift transform oTg
  * standard gripper second lift transform oTg 
  * particular gripper transform gTg
<code lisp>                           
      (when (and object-to-standard-gripper-transform standard-to-particular-gripper-transform)

        (flet ((object-to-standard-gripper->base-to-particular-gripper (object-to-standard-gripper)
                 (when object-to-standard-gripper
                   (let ((base-to-standard-gripper-transform
                           (cram-tf:multiply-transform-stampeds
                            cram-tf:*robot-base-frame* gripper-tool-frame
                            object-transform          ; bTo
                            object-to-standard-gripper ; oTg'
                            :result-as-pose-or-transform :transform))) ; bTo * oTg' = bTg'
                     (cram-tf:multiply-transform-stampeds ; bTg' * g'Tg = bTg
                      cram-tf:*robot-base-frame* gripper-tool-frame
                      base-to-standard-gripper-transform      ; bTg'
                      standard-to-particular-gripper-transform ; g'Tg
                      :result-as-pose-or-transform :pose)))))

          (mapcar #'object-to-standard-gripper->base-to-particular-gripper
                  (list object-to-standard-gripper-pregrasp-transform
                        object-to-standard-gripper-2nd-pregrasp-transform
                        object-to-standard-gripper-transform
                        object-to-standard-gripper-lift-transform
                        object-to-standard-gripper-2nd-lift-transform)))))))



</code>

The last step is to calculate the base-to-standard-gripper-transform so that a transformation from object-to-standard-gripper to base-to-particular-gripper can be done. 
====== Write a Demo======

Depending on what the scenario will be a different demo has to take place. In cram/cram_pr2/cram_pr2_pick_place_demo/ the Demo for pick and place is given. 

All that is needed:

  * Launch file with all important nodes.
  * Resources includes all .stl and .dae files from the spawned object in the scenario.
  * Then a costmap needs to be defined.
  * In projection-demo.lisp all the function that will be called in the demo are implemented e.g. place-object        spawn-objects-on-sink-counter
  * In demo.lisp the demo is written and trainings-data.