From 37ce51d55861ca504c59684de0247a7c6c48cd37 Mon Sep 17 00:00:00 2001 From: Cannon Lewis Date: Mon, 24 Apr 2017 00:25:03 -0500 Subject: [PATCH 1/4] Modified package and method definitions to expose moveit scene file parsing --- lisp/amino-rx.asd | 2 +- lisp/rx/robray-package.lisp | 3 +++ lisp/rx/scenefile/moveit-to-tmkit.lisp | 20 ++++++++++++++++++++ lisp/rx/scenefile/moveit.lisp | 2 +- 4 files changed, 25 insertions(+), 2 deletions(-) create mode 100644 lisp/rx/scenefile/moveit-to-tmkit.lisp diff --git a/lisp/amino-rx.asd b/lisp/amino-rx.asd index d181eb90..56b64e61 100644 --- a/lisp/amino-rx.asd +++ b/lisp/amino-rx.asd @@ -78,7 +78,7 @@ (:file "rx/lexer" :depends-on ("rx/util")) (:file "rx/inex" :depends-on ("rx/util")) (:file "rx/scenefile/curly" :depends-on ("rx/scenegraph" "rx/mesh" "rx/lexer" "rx/inex")) - ;(:file "rx/scenefile/moveit" :depends-on ("rx/scenegraph" "rx/mesh")) + (:file "rx/scenefile/moveit" :depends-on ("rx/scenegraph" "rx/mesh")) (:file "rx/scenefile/scenefile" :depends-on ("rx/scenefile/urdf" "rx/scenefile/curly")) (:file "rx/animate" :depends-on ("rx/scenegraph" "rx/povray")) diff --git a/lisp/rx/robray-package.lisp b/lisp/rx/robray-package.lisp index fec6cff3..91bd6465 100644 --- a/lisp/rx/robray-package.lisp +++ b/lisp/rx/robray-package.lisp @@ -102,4 +102,7 @@ :motion-plan-endpoint-map :motion-plan-endpoint-array + ;; scenegraph parsing + :load-moveit-scene + )) diff --git a/lisp/rx/scenefile/moveit-to-tmkit.lisp b/lisp/rx/scenefile/moveit-to-tmkit.lisp new file mode 100644 index 00000000..273397aa --- /dev/null +++ b/lisp/rx/scenefile/moveit-to-tmkit.lisp @@ -0,0 +1,20 @@ +(load "/home/cannon/tmkit/load.lisp") + +(in-package robray) + +(require :amino-rx) + +(defparameter *robray-tmp-directory* "/tmp/amino-cannon") + +(robray::load-moveit-scene "~/moveit_scene.scene") + + +(with-open-file (stream "~/moveit_scene.robray" + :direction :output + :if-exists :supersede + :if-does-not-exist :create) + (format stream (sycamore:rope-string (sycamore:rope (robray::load-moveit-scene "~/moveit_scene.scene"))))) + + +(TMSMT:TMP-DRIVER :START-SCENE '("package://ur_description/urdf/ur5_robotiq_robot_limited.urdf" "/home/cannon/baxter-blocks/moveit_scene.robray" "/home/cannon/baxter-blocks/ur5_robotiq85_allowed_collision.robray") :GOAL-SCENE '("/home/cannon/baxter-blocks/moveit_scene_goal.robray") :PDDL '("/home/cannon/baxter-blocks/tm-blocks.pddl") :GUI "1" :SCRIPTS '("/home/cannon/baxter-blocks/tm-blocks.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "ur5-robotiq-sussman.tmp" :WRITE-FACTS NIL :MOTION-TIMEOUT NIL :START-PLAN NIL :START NIL :PREFIX-CACHE T :CONSTRAINTS :STATE) + diff --git a/lisp/rx/scenefile/moveit.lisp b/lisp/rx/scenefile/moveit.lisp index 6279c6be..28819913 100644 --- a/lisp/rx/scenefile/moveit.lisp +++ b/lisp/rx/scenefile/moveit.lisp @@ -140,7 +140,7 @@ (vec3 translation)) :geometry (scene-geometry-box (draw-options-default :color (subseq rgba 0 3) :alpha (elt rgba 3)) - (scene-box size)))))) + size))))) (parse-mesh (name) (destructuring-bind (vertex-count face-count) (int-line) From 5c7719d6e60fe5b4fd80dccf7ca4d5ff4460db6e Mon Sep 17 00:00:00 2001 From: Cannon Lewis Date: Mon, 1 May 2017 00:23:12 -0500 Subject: [PATCH 2/4] Moved moveit-to-tmkit files --- lisp/moveit-to-tmkit/moveit-to-tmkit.lisp | 41 +++++++++++++++++++ lisp/moveit-to-tmkit/moveit_scene.robray | 17 ++++++++ lisp/moveit-to-tmkit/moveit_scene.scene | 9 ++++ lisp/moveit-to-tmkit/moveit_scene_goal.robray | 17 ++++++++ lisp/rx/scenefile/moveit-to-tmkit.lisp | 20 --------- 5 files changed, 84 insertions(+), 20 deletions(-) create mode 100644 lisp/moveit-to-tmkit/moveit-to-tmkit.lisp create mode 100644 lisp/moveit-to-tmkit/moveit_scene.robray create mode 100644 lisp/moveit-to-tmkit/moveit_scene.scene create mode 100644 lisp/moveit-to-tmkit/moveit_scene_goal.robray delete mode 100644 lisp/rx/scenefile/moveit-to-tmkit.lisp diff --git a/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp new file mode 100644 index 00000000..51680a17 --- /dev/null +++ b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp @@ -0,0 +1,41 @@ +(load "/home/cannon/tmkit/load.lisp") + +(in-package robray) + +(require :amino-rx) + +(defparameter *robray-tmp-directory* "/tmp/amino") + +(robray::load-moveit-scene "~/moveit_scene.scene") + +;; Loading Moveit scene into robray, with reparenting, etc. +(let* ((moveit-graph (robray::load-moveit-scene "~/moveit_scene.scene")) + (box-frame (scene-graph-find moveit-graph "box1"))) + (progn + (robray::scene-graph-f moveit-graph (scene-frame-fixed nil "null-frame")) + (setf moveit-graph (robray::scene-graph-reparent moveit-graph "null-frame" "box1")) + (scene-graph-remove-frame moveit-graph "box1") + ;; Annotate frame box1 with type moveable + (setf (scene-geometry-type (car (scene-frame-geometry box-frame))) (sycamore:tree-set-insert (scene-geometry-type (car (scene-frame-geometry box-frame))) "moveable")) + (print (draw-option (scene-geometry-options (car (scene-frame-geometry box-frame))) :type)) + (print (scene-graph-find moveit-graph "box1")) + (with-open-file (stream "~/moveit_scene.robray" + :direction :output + :if-exists :supersede + :if-does-not-exist :create) + (format stream (sycamore:rope-string (sycamore:rope moveit-graph)))))) + +;; Testing +(scene-graph-find (load-scene-file "/home/cannon/moveit_scene.robray") "box1") + +;; Just writing to file +(with-open-file (stream "~/moveit_scene.robray" + :direction :output + :if-exists :supersede + :if-does-not-exist :create) + (format stream (sycamore:rope-string (sycamore:rope (robray::load-moveit-scene "~/moveit_scene.scene"))))) + + +;; Full driver run +(TMSMT:TMP-DRIVER :START-SCENE '("package://ur_description/urdf/ur5_robotiq_robot_limited.urdf" "/home/cannon/baxter-blocks/moveit_scene.robray" "/home/cannon/baxter-blocks/ur5_robotiq85_allowed_collision.robray") :GOAL-SCENE '("/home/cannon/moveit_scene_goal.robray") :PDDL '("/home/cannon/baxter-blocks/tm-blocks.pddl") :GUI "1" :SCRIPTS '("/home/cannon/baxter-blocks/tm-blocks.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "ur5-robotiq-sussman.tmp" :WRITE-FACTS NIL :MOTION-TIMEOUT NIL :START-PLAN NIL :START NIL :PREFIX-CACHE T) + diff --git a/lisp/moveit-to-tmkit/moveit_scene.robray b/lisp/moveit-to-tmkit/moveit_scene.robray new file mode 100644 index 00000000..464955da --- /dev/null +++ b/lisp/moveit-to-tmkit/moveit_scene.robray @@ -0,0 +1,17 @@ +frame box1 { + parent null-frame; + type fixed; + translation [0.3490000069141388, -0.2329999953508377, 0.9700000286102295]; + geometry { + shape box; + isa moveable; + dimension [0.07000000029802322, 0.07000000029802322, 0.07000000029802322]; + color [0.0, 0.0, 0.0]; + alpha 1.0; + } +} + +frame null-frame { + type fixed; +} + diff --git a/lisp/moveit-to-tmkit/moveit_scene.scene b/lisp/moveit-to-tmkit/moveit_scene.scene new file mode 100644 index 00000000..fc30c97a --- /dev/null +++ b/lisp/moveit-to-tmkit/moveit_scene.scene @@ -0,0 +1,9 @@ +(noname)+ +* box1 +1 +box +0.07 0.07 0.07 +0.349 -0.233 0.97 +0 0 0 1 +0 0 0 0 +. diff --git a/lisp/moveit-to-tmkit/moveit_scene_goal.robray b/lisp/moveit-to-tmkit/moveit_scene_goal.robray new file mode 100644 index 00000000..464955da --- /dev/null +++ b/lisp/moveit-to-tmkit/moveit_scene_goal.robray @@ -0,0 +1,17 @@ +frame box1 { + parent null-frame; + type fixed; + translation [0.3490000069141388, -0.2329999953508377, 0.9700000286102295]; + geometry { + shape box; + isa moveable; + dimension [0.07000000029802322, 0.07000000029802322, 0.07000000029802322]; + color [0.0, 0.0, 0.0]; + alpha 1.0; + } +} + +frame null-frame { + type fixed; +} + diff --git a/lisp/rx/scenefile/moveit-to-tmkit.lisp b/lisp/rx/scenefile/moveit-to-tmkit.lisp deleted file mode 100644 index 273397aa..00000000 --- a/lisp/rx/scenefile/moveit-to-tmkit.lisp +++ /dev/null @@ -1,20 +0,0 @@ -(load "/home/cannon/tmkit/load.lisp") - -(in-package robray) - -(require :amino-rx) - -(defparameter *robray-tmp-directory* "/tmp/amino-cannon") - -(robray::load-moveit-scene "~/moveit_scene.scene") - - -(with-open-file (stream "~/moveit_scene.robray" - :direction :output - :if-exists :supersede - :if-does-not-exist :create) - (format stream (sycamore:rope-string (sycamore:rope (robray::load-moveit-scene "~/moveit_scene.scene"))))) - - -(TMSMT:TMP-DRIVER :START-SCENE '("package://ur_description/urdf/ur5_robotiq_robot_limited.urdf" "/home/cannon/baxter-blocks/moveit_scene.robray" "/home/cannon/baxter-blocks/ur5_robotiq85_allowed_collision.robray") :GOAL-SCENE '("/home/cannon/baxter-blocks/moveit_scene_goal.robray") :PDDL '("/home/cannon/baxter-blocks/tm-blocks.pddl") :GUI "1" :SCRIPTS '("/home/cannon/baxter-blocks/tm-blocks.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "ur5-robotiq-sussman.tmp" :WRITE-FACTS NIL :MOTION-TIMEOUT NIL :START-PLAN NIL :START NIL :PREFIX-CACHE T :CONSTRAINTS :STATE) - From f3ed1a2c9dfcb83d326a54d18ccd1264c9f6e787 Mon Sep 17 00:00:00 2001 From: Cannon Lewis Date: Tue, 2 May 2017 15:02:20 -0500 Subject: [PATCH 3/4] Cleaned up scene stuff --- lisp/moveit-to-tmkit/moveit-to-tmkit.lisp | 48 ++++++------------- lisp/moveit-to-tmkit/moveit_scene.robray | 17 ------- lisp/moveit-to-tmkit/moveit_scene.scene | 9 ---- lisp/moveit-to-tmkit/moveit_scene_goal.robray | 17 ------- 4 files changed, 15 insertions(+), 76 deletions(-) delete mode 100644 lisp/moveit-to-tmkit/moveit_scene.robray delete mode 100644 lisp/moveit-to-tmkit/moveit_scene.scene delete mode 100644 lisp/moveit-to-tmkit/moveit_scene_goal.robray diff --git a/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp index 51680a17..7da24ad7 100644 --- a/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp +++ b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp @@ -1,4 +1,4 @@ -(load "/home/cannon/tmkit/load.lisp") +(load "~/tmkit/load.lisp") (in-package robray) @@ -6,36 +6,18 @@ (defparameter *robray-tmp-directory* "/tmp/amino") -(robray::load-moveit-scene "~/moveit_scene.scene") - ;; Loading Moveit scene into robray, with reparenting, etc. -(let* ((moveit-graph (robray::load-moveit-scene "~/moveit_scene.scene")) - (box-frame (scene-graph-find moveit-graph "box1"))) - (progn - (robray::scene-graph-f moveit-graph (scene-frame-fixed nil "null-frame")) - (setf moveit-graph (robray::scene-graph-reparent moveit-graph "null-frame" "box1")) - (scene-graph-remove-frame moveit-graph "box1") - ;; Annotate frame box1 with type moveable - (setf (scene-geometry-type (car (scene-frame-geometry box-frame))) (sycamore:tree-set-insert (scene-geometry-type (car (scene-frame-geometry box-frame))) "moveable")) - (print (draw-option (scene-geometry-options (car (scene-frame-geometry box-frame))) :type)) - (print (scene-graph-find moveit-graph "box1")) - (with-open-file (stream "~/moveit_scene.robray" - :direction :output - :if-exists :supersede - :if-does-not-exist :create) - (format stream (sycamore:rope-string (sycamore:rope moveit-graph)))))) - -;; Testing -(scene-graph-find (load-scene-file "/home/cannon/moveit_scene.robray") "box1") - -;; Just writing to file -(with-open-file (stream "~/moveit_scene.robray" - :direction :output - :if-exists :supersede - :if-does-not-exist :create) - (format stream (sycamore:rope-string (sycamore:rope (robray::load-moveit-scene "~/moveit_scene.scene"))))) - - -;; Full driver run -(TMSMT:TMP-DRIVER :START-SCENE '("package://ur_description/urdf/ur5_robotiq_robot_limited.urdf" "/home/cannon/baxter-blocks/moveit_scene.robray" "/home/cannon/baxter-blocks/ur5_robotiq85_allowed_collision.robray") :GOAL-SCENE '("/home/cannon/moveit_scene_goal.robray") :PDDL '("/home/cannon/baxter-blocks/tm-blocks.pddl") :GUI "1" :SCRIPTS '("/home/cannon/baxter-blocks/tm-blocks.py") :VERBOSE NIL :MAX-STEPS 10 :OUTPUT "ur5-robotiq-sussman.tmp" :WRITE-FACTS NIL :MOTION-TIMEOUT NIL :START-PLAN NIL :START NIL :PREFIX-CACHE T) - +(defun moveit-to-tmkit (moveit-file &optional (out-file "~/out.robray")) + (let* ((moveit-graph (robray::load-moveit-scene moveit-file)) + (box-frame (scene-graph-find moveit-graph "box1"))) + (progn + (robray::scene-graph-f moveit-graph (scene-frame-fixed nil "null-frame")) + (setf moveit-graph (robray::scene-graph-reparent moveit-graph "null-frame" "box1")) + (scene-graph-remove-frame moveit-graph "box1") + ;; Annotate frame box1 with type moveable + (setf (scene-geometry-type (car (scene-frame-geometry box-frame))) (sycamore:tree-set-insert (scene-geometry-type (car (scene-frame-geometry box-frame))) "moveable")) + (with-open-file (stream out-file + :direction :output + :if-exists :supersede + :if-does-not-exist :create) + (format stream (sycamore:rope-string (sycamore:rope moveit-graph))))))) diff --git a/lisp/moveit-to-tmkit/moveit_scene.robray b/lisp/moveit-to-tmkit/moveit_scene.robray deleted file mode 100644 index 464955da..00000000 --- a/lisp/moveit-to-tmkit/moveit_scene.robray +++ /dev/null @@ -1,17 +0,0 @@ -frame box1 { - parent null-frame; - type fixed; - translation [0.3490000069141388, -0.2329999953508377, 0.9700000286102295]; - geometry { - shape box; - isa moveable; - dimension [0.07000000029802322, 0.07000000029802322, 0.07000000029802322]; - color [0.0, 0.0, 0.0]; - alpha 1.0; - } -} - -frame null-frame { - type fixed; -} - diff --git a/lisp/moveit-to-tmkit/moveit_scene.scene b/lisp/moveit-to-tmkit/moveit_scene.scene deleted file mode 100644 index fc30c97a..00000000 --- a/lisp/moveit-to-tmkit/moveit_scene.scene +++ /dev/null @@ -1,9 +0,0 @@ -(noname)+ -* box1 -1 -box -0.07 0.07 0.07 -0.349 -0.233 0.97 -0 0 0 1 -0 0 0 0 -. diff --git a/lisp/moveit-to-tmkit/moveit_scene_goal.robray b/lisp/moveit-to-tmkit/moveit_scene_goal.robray deleted file mode 100644 index 464955da..00000000 --- a/lisp/moveit-to-tmkit/moveit_scene_goal.robray +++ /dev/null @@ -1,17 +0,0 @@ -frame box1 { - parent null-frame; - type fixed; - translation [0.3490000069141388, -0.2329999953508377, 0.9700000286102295]; - geometry { - shape box; - isa moveable; - dimension [0.07000000029802322, 0.07000000029802322, 0.07000000029802322]; - color [0.0, 0.0, 0.0]; - alpha 1.0; - } -} - -frame null-frame { - type fixed; -} - From 421dc6b40fe1800f0c78df63eee432e24ce89d84 Mon Sep 17 00:00:00 2001 From: Cannon Lewis Date: Tue, 2 May 2017 18:10:34 -0500 Subject: [PATCH 4/4] Fixed reparenting to not be box1 specific --- lisp/moveit-to-tmkit/moveit-to-tmkit.lisp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp index 7da24ad7..b5e38021 100644 --- a/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp +++ b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp @@ -6,16 +6,16 @@ (defparameter *robray-tmp-directory* "/tmp/amino") +(defun reparent (parent-frame frame scene-graph) + (setf scene-graph (robray::scene-graph-reparent scene-graph parent-frame frame))) + ;; Loading Moveit scene into robray, with reparenting, etc. (defun moveit-to-tmkit (moveit-file &optional (out-file "~/out.robray")) - (let* ((moveit-graph (robray::load-moveit-scene moveit-file)) - (box-frame (scene-graph-find moveit-graph "box1"))) + (let* ((moveit-graph (robray::load-moveit-scene moveit-file))) (progn (robray::scene-graph-f moveit-graph (scene-frame-fixed nil "null-frame")) - (setf moveit-graph (robray::scene-graph-reparent moveit-graph "null-frame" "box1")) - (scene-graph-remove-frame moveit-graph "box1") + (robray::map-scene-graph-frames nil #'(lambda (frame) (setf moveit-graph (robray::reparent "null-frame" (robray::scene-frame-name frame) moveit-graph))) moveit-graph) ;; Annotate frame box1 with type moveable - (setf (scene-geometry-type (car (scene-frame-geometry box-frame))) (sycamore:tree-set-insert (scene-geometry-type (car (scene-frame-geometry box-frame))) "moveable")) (with-open-file (stream out-file :direction :output :if-exists :supersede