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:20] – [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))
  
 +    ;; First correct the object transform such that rotationally-symmetric objects
 +    ;; would not be grasped in an awkward way with weird orientations
 +    (when (prolog `(object-rotationally-symmetric ,object-type))
 +      (setf object-transform
 +            (cram-tf:copy-transform-stamped
 +             object-transform
 +             :rotation (cl-transforms:make-identity-rotation))))
 +
 +    (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))))))))
 +      (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======