This is an old revision of the document!


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 cram-designator, high_level_plans and cram_prolog_reasoning.

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:

;;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))))
              [...]

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.

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

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.

 (exe:perform
     (desig:an action
               (type setting-gripper)
               (gripper ?arm)
               (position ?gripper-opening)))

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

;;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)))))
               [...]
 

Pick-action-designator

Get-trajectory

Write a Demo