Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
Next revisionBoth sides next revision
tutorials:intermediate:new_actions [2019/01/02 16:17] – [Pick-action-designator] vanessatutorials:intermediate:new_actions [2019/01/02 16:26] – [Get-trajectory] vanessa
Line 227: Line 227:
 ====== Get-trajectory ====== ====== 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>
 ====== Write a Demo====== ====== Write a Demo======