Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revisionNext revisionBoth sides next revision | ||
tutorials:intermediate:new_actions [2018/12/30 15:42] – [Pick-action-designator] vanessa | tutorials:intermediate:new_actions [2019/01/02 16:26] – [Get-trajectory] vanessa | ||
---|---|---|---|
Line 166: | Line 166: | ||
====== Pick-action-designator ====== | ====== Pick-action-designator ====== | ||
- | TODO: Write when the new manipulation interface is done. | ||
+ | |||
+ | Now it is necessary to define a action designator of the new type for pick-up e.g. | ||
+ | |||
+ | <code lisp> | ||
+ | ;; | ||
+ | (def-fact-group pick-and-place-plans (desig: | ||
+ | (<- (desig: | ||
+ | ? | ||
+ | ? | ||
+ | ? | ||
+ | ? | ||
+ | ;; extract info from ? | ||
+ | (spec: | ||
+ | (spec: | ||
+ | (desig: | ||
+ | (spec: | ||
+ | (spec: | ||
+ | (-> (spec: | ||
+ | (true) | ||
+ | (man-int: | ||
+ | (lisp-fun man-int: | ||
+ | ;; infer missing information like ?grasp type, gripping ? | ||
+ | (lisp-fun man-int: | ||
+ | (-> (man-int: | ||
+ | (equal ? | ||
+ | (equal ? | ||
+ | (-> (spec: | ||
+ | (true) | ||
+ | (and (lisp-fun man-int: | ||
+ | | ||
+ | (lisp-fun man-int: | ||
+ | (lisp-fun man-int: | ||
+ | (lisp-fun man-int: | ||
+ | ? | ||
+ | ? | ||
+ | (lisp-fun man-int: | ||
+ | ? | ||
+ | ? | ||
+ | (lisp-fun extract-pick-up-manipulation-poses ?arm ?left-poses ? | ||
+ | (? | ||
+ | ? | ||
+ | ? | ||
+ | |||
+ | | ||
+ | </ | ||
+ | |||
+ | |||
+ | |||
+ | First all of the information from the ? | ||
+ | (-> (man-int: | ||
+ | (equal ? | ||
+ | (equal ? | ||
+ | (-> (spec: | ||
+ | (true) | ||
+ | (and (lisp-fun man-int: | ||
+ | | ||
+ | |||
+ | 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. | ||
====== Get-trajectory ====== | ====== Get-trajectory ====== | ||
+ | |||
+ | <code lisp> | ||
+ | ;; | ||
+ | (defgeneric get-object-grasping-poses (object-name object-type arm grasp object-transform) | ||
+ | (: | ||
+ | (:method (object-name object-type arm grasp object-transform) | ||
+ | (declare (type symbol object-name object-type arm grasp) | ||
+ | (type cl-transforms-stamped: | ||
+ | |||
+ | (when (prolog `(object-rotationally-symmetric , | ||
+ | (setf object-transform | ||
+ | (cram-tf: | ||
+ | | ||
+ | : | ||
+ | </ | ||
+ | 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 | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | object-type object-name arm grasp | ||
+ | object-to-standard-gripper-transform)) | ||
+ | | ||
+ | | ||
+ | object-type object-name arm grasp | ||
+ | object-to-standard-gripper-transform)) | ||
+ | | ||
+ | | ||
+ | object-type object-name arm grasp | ||
+ | object-to-standard-gripper-transform)) | ||
+ | | ||
+ | | ||
+ | object-type object-name arm grasp | ||
+ | object-to-standard-gripper-transform)) | ||
+ | | ||
+ | | ||
+ | gripper-tool-frame | ||
+ | gripper-tool-frame | ||
+ | 0.0 | ||
+ | (cut: | ||
+ | '? | ||
+ | (car (prolog: | ||
+ | `(and (cram-robot-interfaces: | ||
+ | | ||
+ | ?robot ? | ||
+ | | ||
+ | </ | ||
+ | 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-> | ||
+ | (when object-to-standard-gripper | ||
+ | (let ((base-to-standard-gripper-transform | ||
+ | | ||
+ | cram-tf: | ||
+ | object-transform | ||
+ | object-to-standard-gripper ; oTg' | ||
+ | : | ||
+ | | ||
+ | cram-tf: | ||
+ | base-to-standard-gripper-transform | ||
+ | standard-to-particular-gripper-transform ; g'Tg | ||
+ | : | ||
+ | |||
+ | (mapcar #' | ||
+ | (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))))))) | ||
+ | |||
+ | </ | ||
====== Write a Demo====== | ====== Write a Demo====== |