Modified package and method definitions to expose moveit scene file p…

…arsing
golems · Apr 24, 2017 · 37ce51d · 37ce51d
1 parent a2f9146
commit 37ce51d
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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)