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

(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