Created
November 27, 2019 04:11
-
-
Save k-okada/28aa1d47c0216dd888e432f7d5edfc5b to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
(require :pr2-interface "package://pr2eus/pr2-interface.l") | |
(require :pr2eus-moveit "package://pr2eus_moveit/euslisp/pr2eus-moveit.l") | |
(load "package://pr2eus_moveit/tutorials/collision-object-sample.l") | |
;; pr2eus_moveit | |
(defun pr2-moveit-demo () | |
(let (cds ret) | |
(pr2-init) | |
(send *pr2* :angle-vector (send *ri* :state :reference-vector)) | |
(objects (list *pr2*)) | |
(setq *me* (instance pr2-moveit-environment :init)) | |
(send *me* :robot :angle-vector (send *ri* :state :reference-vector)) | |
(setq cds (send *me* :robot :larm :end-coords :copy-worldcoords)) | |
(send cds :translate #f(0 0 -100) :world) | |
(send cds :draw-on :flush t :size 100) | |
(send *me* :get-ik-for-pose cds :larm :end-coords (list :larm :end-coords)) | |
(setq ret (send *me* :motion-plan :larm)) | |
(send *me* :execute-trajectory (send ret :trajectory)) ;; FIX ME IT DOES NOT WORK... [ERROR] [1574826354.919305258, 274.079000000]: attempted to call service /execute_kinematic_path, but failed | |
)) | |
;; moveit_demo | |
(defun pr2-moveit-collision-demo () | |
(let (ret) | |
(pr2-init) | |
(init-collision-demo *pr2* "base_link") | |
(pub-objects) | |
(objects (append (list *pr2*) (objects))) | |
(send *pr2* :angle-vector (send *ri* :state :reference-vector)) | |
(send *irtviewer* :draw-objects) | |
;; | |
(setq larm-target #s(coordinates plist nil rot #2f((0.968121 0.247905 0.035856) (-0.247379 0.968742 -0.018505) (-0.039323 0.009045 0.999186)) pos #f(-81.4698 172.586 808.425))) | |
(setq *me* (instance pr2-moveit-environment :init)) | |
(send *me* :sync-robot-model *pr2*) | |
(send *me* :get-ik-for-pose larm-target :larm :end-coords (list :larm :end-coords)) | |
(setq ret (send *me* :motion-plan :larm)) | |
(send *me* :execute-trajectory (send ret :trajectory)) | |
;; | |
;; OR | |
;; | |
;; execute plan in rviz | |
;; | |
;; | |
(send *pr2* :angle-vector (send *ri* :state :reference-vector)) | |
(send *irtviewer* :draw-objects) | |
(sync-larm) | |
(send *ri* :start-grasp :larm) | |
(send *ri* :angle-vector (send *pr2* :angle-vector) 2000) | |
(send *pr2* :angle-vector (send *ri* :state :reference-vector)) | |
(send *pr2* :larm :inverse-kinematics | |
(send (send *pr2* :larm :end-coords :copy-worldcoords) :translate #f(0 0 100) :world)) | |
(send *irtviewer* :draw-objects) | |
(send *ri* :angle-vector (send *pr2* :angle-vector) 3000) | |
)) | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment