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/moveit-to-tmkit/moveit-to-tmkit.lisp b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp new file mode 100644 index 00000000..b5e38021 --- /dev/null +++ b/lisp/moveit-to-tmkit/moveit-to-tmkit.lisp @@ -0,0 +1,23 @@ +(load "~/tmkit/load.lisp") + +(in-package robray) + +(require :amino-rx) + +(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))) + (progn + (robray::scene-graph-f moveit-graph (scene-frame-fixed nil "null-frame")) + (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 + (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/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.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)