Actions
Holding behaves like picking-up, but does no lift motion after the object was grasped. Actually, inside of CRAM, after the action of type holding was resolved the pick-up method in cram-mobile-pick-place-plans is called with the key :hold being T.
Holding needs the object it should hold, which arm it should use and how it should orientate its gripper to hold the object specified with the keywords :left-hold or :right-hold. The poses of these grasps have to be definied in the CRAM package cram-object-knowledge for each object type. The holding action is called like this:
(exe:perform (desig:an action
(type holding)
(object ?object-to-slice)
(arm ?arm-to-hold)
(grasp ?grasp-to-hold)))
More examples of this action can be found in the demo.lisp of cram_pr2_cut_pour_demo.
Slicing is implemented as a chain of approaching motions, which are calculated during execution in cram-manipulation-interfaces::get-action-trajectory methods.
Slicing needs the object to slice since its bounding box is used to calculate how often it should be sliced. Moreover, it needs the arm which holds the object during the slicing process. The start slice-position is currently specified with the keywords :right-top or :left-top. right-top indicates the object should be sliced from the top right to the left (vice-versa for left-top).
This action needs for every object it should slice concrete poses in its object frame which specify where each slicing process starts and ends. These slicing poses will then be used with an offset n times to slice the object in n pieces.
The object designator ?object-to-slice needs to hold the objects type, name and pose for the successful calculation of the slicing trajectory. Furthermore, the slicing actions allows to pass a collision-mode too (:avoid-all, :allow-all, :allow-hand). The holding action is called like this:
(exe:perform (desig:an action
(type slicing)
(object ?object-to-slice)
(arm ?arm-slicing-with)
(grasp ?slice-position)))
More examples of this action can be found in the demo.lisp of cram_pr2_cut_pour_demo.
Pouring is implemented as a chain of approach and tilting events, which in the end make it possible to move arms in sequence. The tilting rotates the object from which should be poured from always around 100 degrees.
Pouring needs the object to pour into, the arms which should be used for this action and from which side the object should be filled from, which is passed with the paramter grasp (:left-side, :right-side, :front, :back). The object designator ?object-to-pour-into needs to hold the objects type, name and pose for the successful calculation of the pouring trajectory.
First, the robot approaches the object ?object-to-pour-into with the object it holds in its arm(s). From this pose the tilting motion is initiated. Since this pose depends on the object ?object-to-pour-into's size and from which side it should be filled from, one has to specify these approach poses. The translation in this approach pose is used as an offset from the object and is therefore applied in robot frame on the objects position. The orientation of the approach poses is used to define the orientation of the gripper in the robot frame.
The parameter in grasp depends on the robot position. Therefore, the grasp :front matches with the x-coordinate of the robot and results in pouring from the direction the robot base is facing to (:left-side matches with the y-coordinate of the robot, :back with the -x-coordinate, right-side with the -y-coordinate). Due to that, the objects orientation does not matter for pouring. To summarize, for pouring from the left one should like in the example pass :left-side; for pouring from the right, :right-side; for pouring from the back, :back; and for pouring from the front, :front. Furthermore, the pouring actions allows to pass a collision-mode too (:avoid-all, :allow-all, :allow-hand).
(desig:an action
(type pouring)
(object ?object-to-pour-into)
(arms (left))
(grasp left-side))
More examples of this action can be found in the demo.lisp of cram_pr2_cut_pour_demo and cram_pr2_popcorn_demo.
(urdf-proj:with-projected-robot
(let ((?arm :left)
(?container-desig
(an object
(type drawer)
(urdf-name sink-area-left-middle-drawer-main)
(part-of kitchen))))
(exe:perform (an action
(type navigating)
(location
(a location
(reachable-for pr2)
(arm ?arm)
(object ?container-desig)))))))
(urdf-proj:with-projected-robot
(perform (an action
(type opening)
(arm right)
(object (an object
(type drawer)
(urdf-name sink-area-left-upper-drawer-main)
(part-of kitchen)))
(distance 0.3))))
(urdf-proj:with-simulated-robot
(exe:perform
(an action
(type opening)
(arm right)
(distance 0.6)
(object
(an object
(type fridge)
(urdf-name iai-fridge-door)
(part-of kitchen))))))
(urdf-proj:with-simulated-robot
(exe:perform
(an action
(type accessing)
(arm right)
(distance 0.4)
(location
(a location
(in
(an object
(type container-revolute)
(urdf-name iai-fridge-door)
(part-of kitchen))))))))
- There are 3 types of joints: :FIXED, :PRISMATIC, :REVOLUTE
- so when we find a joint that's not :FIXED we know it's the joint we are interested in to manipulate
- Connecting joints (those which are revolute or prismatic and connect a container with the rest of the environment)
- Have proper limits set
- Are above their container-link in the URDF hierarchy
- That's why for opening the fridge one has to open the fridge-door instead of the fridge itself
- Handles
- Handle-links have the string "handle" in their name
- Otherwise error in find-handle-under-link
- Prismatic containers have a horizontal handle
- Revolute containers have a vertical handle
- Exceptions in the iai-kitchen:
- "oven_area_area_left_drawer_main"
- "oven_area_area_right_drawer_main"
- both are prismatic but have vertical handles
- Handle-links have the string "handle" in their name
- Rotation axis for revolute joints is Z (in world)
- Every container-link has a rigid-body with a pose
- A container's URDF joints don't branch. (Only the first "to-joint" of an link is followed when going down the hierarchy to find a handle or joint-type)
(exe:perform
(an action
(type transporting)
(object ?object)
(context :table-setting)
;; (grasps (:back :top :front))
(desig:when ?arm
(arms (?arm)))
;; (desig:when ?grasp
;; (grasp ?grasp))
(target ?deliver-location)
))
The designator is resolved in this order:
- type: navigation to location (go-without-collisions)
- type: turning-towards
- type: accessing/sealing (it is resolved by location with :in, :above context)
- type: search-for-object
- type: fetching, contains this:
- object
- arms
- grasp
- robot-location
- look-location
- pick-up-action
- type: deliver, contains this:
- object
- arm
- context
- target
- target stable?
- robot-location
- place-action
- type: transport (same as deliver just without place)
- defun go-without-collisions
- defun turn-toward
- defun manipulate enviroment
- defun fetch
- defun deliver
- defun drop-at-sink
- defun transport
If you want to add a new context you'll have to change/add the following files accordingly.
- ==cram_knowrob/cram_knowrob_vr/src/queriess.lisp==
- query-name-and-surface-T-object-by-object-type (l/279)
- query-surface-name-dim-and-surface-T-object-by-object-type (l/345)
- query-name-and-surface-T-camera-by-object-type (l/415)
;;line:
(case context
;; if we're setting a table, the source ("Start") should not be island,
;; and destination ("End") should be island
(:table-setting
(if (equal start-or-end "Start")
", not(owl_subclass_of(SurfaceType, knowrob:'IslandArea'))."
", owl_subclass_of(SurfaceType, knowrob:'IslandArea')."))
;; in case of table cleaning it's the other way around:
;; source "Start" should be island, destination "End" should not be island
(:table-cleaning
(if (equal start-or-end "Start")
", owl_subclass_of(SurfaceType, knowrob:'IslandArea')."
", not(owl_subclass_of(SurfaceType, knowrob:'IslandArea'))."))
- ==cram_demos/cram_object_knowledge/src/household.lisp==
- make-locations example:
;;;;;;;; fetching locations
(mapcar (lambda (type)
(defmethod man-int:get-object-likely-location :heuristics 20
((object-type (eql type))
environment human
(context (eql :table-setting-counter)))
(make-location-on-sink-left-front environment)))
'(:plate))
;;;;;;;;; destination locations
(mapcar (lambda (object-type)
(defmethod man-int:get-object-destination :heuristics 20
((object-type (eql object-type))
environment human
(context (eql :table-setting-counter)))
(make-location-on-kitchen-island-slots object-type environment)))
'(:bowl :plate))
;;;;;;;; kitchen island location
(defun make-location-on-kitchen-island-slots (?object-type ?environment-name)
(desig:a location
(on (desig:an object
(type counter-top)
(urdf-name kitchen-island-surface)
(owl-name "kitchen_island_counter_top")
(part-of ?environment-name)))
(for (desig:an object (type ?object-type)))
(side back)
(side right)
(range-invert 0.5)
(context table-setting)
(object-count 3)))
:::warning Finally, get-object-destination of :table-setting is no the same as object-likely-location of :table-cleaning, as during setting we are relative to another object, and during cleaning that object might already be taken away. :::
For defining a new grasping pose on object you'll need to consider how the gripper is turned. Please take a look on the following picture.
The Parameter are the following:
;; back / front
(defparameter *x-across-z-grasp-rotation*
'(( 0 0 -1)
(-1 0 0)
( 0 1 0)))
(defparameter *x-across-z-grasp-rotation-2*
'((0 0 -1)
(1 0 0)
(0 -1 0)))
(defparameter *-x-across-z-grasp-rotation*
'((0 0 1)
(1 0 0)
(0 1 0)))
(defparameter *-x-across-z-grasp-rotation-2*
'(( 0 0 1)
(-1 0 0)
( 0 -1 0)))
(defparameter *x-across-y-grasp-rotation*
'(( 0 0 -1)
( 0 -1 0)
(-1 0 0)))
(defparameter *-x-across-y-grasp-rotation*
'(( 0 0 1)
( 0 1 0)
(-1 0 0)))
(defparameter *-x-across-y-grasp-rotation*
'(( 0 0 1)
( 0 1 0)
(-1 0 0)))
;; side
(defparameter *y-across-z-grasp-rotation*
'((1 0 0)
(0 0 -1)
(0 1 0)))
(defparameter *-y-across-z-grasp-rotation*
'((-1 0 0)
( 0 0 1)
( 0 1 0)))
(defparameter *y-across-z-flipped-grasp-rotation*
'((-1 0 0)
( 0 0 -1)
( 0 -1 0)))
(defparameter *-y-across-z-flipped-grasp-rotation*
'((1 0 0)
(0 0 1)
(0 -1 0)))
(defparameter *y-across-x-grasp-rotation*
'((0 1 0)
(0 0 -1)
(-1 0 0)))
(defparameter *-y-across-x-grasp-rotation*
'((0 -1 0)
(0 0 1)
(-1 0 0)))
;; top
(defparameter *z-across-x-grasp-rotation*
'((0 1 0)
(1 0 0)
(0 0 -1)))
(defparameter *z-across-y-grasp-rotation*
'((1 0 0)
(0 -1 0)
(0 0 -1)))
(defparameter *z-diagonal-grasp-rotation*
`((,*-sin-pi/4* ,*sin-pi/4* 0)
(,*sin-pi/4* ,*sin-pi/4* 0)
(0 0 -1)))
;; bottom
(defparameter *-z-across-x-grasp-rotation*
'((0 -1 0)
(1 0 0)
(0 0 1)))