From ee49b6a27f2b2e0f393535bf13f3e32ec5f408c1 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 27 Jul 2015 15:35:03 +0900 Subject: [PATCH 1/3] add irtstl.l, irtwrl.l : copied from wrl2eus.l and read-stl.l --- irteus/Makefile | 4 +- irteus/compile_irtg.l | 2 + irteus/irtext.l | 2 +- irteus/irtstl.l | 428 +++++++++++++++++++++++++++++++++++++++++ irteus/irtwrl.l | 120 ++++++++++++ irteus/test/test-cad.l | 66 +++++++ 6 files changed, 620 insertions(+), 2 deletions(-) create mode 100644 irteus/irtstl.l create mode 100644 irteus/irtwrl.l create mode 100644 irteus/test/test-cad.l diff --git a/irteus/Makefile b/irteus/Makefile index 4123bff84..4dd7348e5 100644 --- a/irteus/Makefile +++ b/irteus/Makefile @@ -73,7 +73,7 @@ MODULES.L=irt_modules.l EUSLIB_MODULES.L=$(addprefix $(EUSDIR)/lib/,$(MODULES.L)) IRTEUS=irtmath irtutil irtgraph pgsql time -IRTEUSG=irtgeo pqp bullet irtcollision irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud +IRTEUSG=irtgeo pqp bullet irtcollision irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud irtstl irtwrl IRTEUSX=irtx IRTEUSIMG=irtimage eusjpeg png IRTEUSGL=irtgl irtglrgb irtviewer @@ -230,6 +230,8 @@ $(INSTALLOBJDIR)/irtscene.$(OSFX): irtscene.l $(INSTALLOBJDIR)/irtmodel.$(OSFX): irtmodel.l $(INSTALLOBJDIR)/irtdyna.$(OSFX): irtdyna.l $(INSTALLOBJDIR)/irtcollada.$(OSFX): irtcollada.l +$(INSTALLOBJDIR)/irtstl.$(OSFX): irtstl.l +$(INSTALLOBJDIR)/irtwrl.$(OSFX): irtwrl.l $(INSTALLOBJDIR)/irtsensor.$(OSFX): irtsensor.l $(INSTALLOBJDIR)/irtpointcloud.$(OSFX): irtpointcloud.l $(INSTALLOBJDIR)/irtrobot.$(OSFX): irtrobot.l diff --git a/irteus/compile_irtg.l b/irteus/compile_irtg.l index 7d081bb5a..992e6c27d 100644 --- a/irteus/compile_irtg.l +++ b/irteus/compile_irtg.l @@ -40,6 +40,8 @@ (comp:compile-file-if-src-newer "irtrobot.l" user::*objdir*) (comp:compile-file-if-src-newer "irtbvh.l" user::*objdir*) (comp:compile-file-if-src-newer "irtcollada.l" user::*objdir*) +(comp:compile-file-if-src-newer "irtstl.l" user::*objdir*) +(comp:compile-file-if-src-newer "irtwrl.l" user::*objdir*) (comp:compile-file-if-src-newer "irtpointcloud.l" user::*objdir*) (exit 0) diff --git a/irteus/irtext.l b/irteus/irtext.l index 977e8e760..af3c5b53a 100644 --- a/irteus/irtext.l +++ b/irteus/irtext.l @@ -34,7 +34,7 @@ (load-library (format nil "~A~A/lib/libirteusg" *eusdir* (unix:getenv "ARCHDIR")) - '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada" "irtpointcloud" "eusbullet" "bullet" "irtcollision")) + '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada" "irtstl" "irtwrl" "irtpointcloud" "eusbullet" "bullet" "irtcollision")) (in-package "USER") (import '(collada::convert-irtmodel-to-collada collada::eus2collada))) (defun load-irteusx () diff --git a/irteus/irtstl.l b/irteus/irtstl.l new file mode 100644 index 000000000..edec87592 --- /dev/null +++ b/irteus/irtstl.l @@ -0,0 +1,428 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; $Id: $ +;;; +;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +;;; +;;; This software is a collection of EusLisp code for robot applications, +;;; which has been developed by the JSK Laboratory for the IRT project. +;;; For more information on EusLisp and its application to the robotics, +;;; please refer to the following papers. +;;; +;;; Toshihiro Matsui +;;; Multithread object-oriented language euslisp for parallel and +;;; asynchronous programming in robotics +;;; Workshop on Concurrent Object-based Systems, +;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +;;; +;;; Permission to use this software for educational, research +;;; and non-profit purposes, without fee, and without a written +;;; agreement is hereby granted to all researchers working on +;;; the IRT project at the University of Tokyo, provided that the +;;; above copyright notice remains intact. +;;; + +(in-package "GEOMETRY") + +(export '(stl2eus eus2stl)) + +;; +;; converting .stl file format object to eus +;; +(defun stl2eus (fname &key debug (color #f(0.8 0.8 0.8)) + (coords) (scale 1000) (name "stlobject") + (use-edge t) (use-normal nil) + (return-type :faceset)) + (let ((str (make-string 80)) + afaceset fd size faces edges normals + ht-norms ht-edgs ht-vrtx tmp vert-lst aface vs) + + (setq ht-norms (make-hash-table :test #'equal)) + (setq ht-edgs (make-hash-table :test #'equal)) + (setq ht-vrtx (make-hash-table :test #'equal)) + + (with-open-file + (f fname) + + (setq fd (send f :infd)) + (unix:uread fd str 80) + + (unix:uread fd str 4) + (setq size (sys:peek str 0 :integer)) + + (dotimes (i size) + (setq tmp nil) + (dotimes (j 4) ;; normal, vertex x 3 + (unix:uread fd str 12) + (push (float-vector (sys:peek str 0 :float) + (sys:peek str 4 :float) + (sys:peek str 8 :float)) tmp) + ) + (unix:uread fd str 2) + (nreverse tmp) + + (setq vert-lst (mapcar #'(lambda (v) + (if scale (scale scale v v)) + (if coords (send coords :rotate-vector v) v)) + (cdr tmp))) + + ;; 2010/05/28 k-okada + ;; vertices in (send :faces) must be exactry same object + ;; with (send (elt v pos) threshould) + (push f ret0) + (push f ret1)) + )) + (list ret0 ret1) + )) + +(defun make-boundingbox-from-faces (fcs) + (let (min-lst max-lst) + (dolist (f fcs) + (push (apply #'vmax (send f :vertices)) max-lst) + (push (apply #'vmin (send f :vertices)) min-lst)) + (list (safe-vmax max-lst) + (safe-vmin min-lst)) + )) + +(defun small-square-axis (b-box) + (let* ((v (apply #'v- b-box)) + (sz (* (elt_x v) (elt_y v))) + (sy (* (elt_x v) (elt_z v))) + (sx (* (elt_y v) (elt_z v))) + (lst (list sx sy sz)) + ) + (position (apply #'min lst) lst :test #'=) + )) + +(defun make-devided-faces-lists (fcs &key (size 32000)) + (let ((fcs-lst (list fcs))) + (while (> (length (car fcs-lst)) size) + (let* ((b-box (make-boundingbox-from-faces (car fcs-lst))) + (pos (small-square-axis b-box)) + (dvs (devide-faces (car fcs-lst) + :threshould (/ (elt (apply #'v+ b-box) pos) 2.0) + :axis pos))) + (setq fcs-lst (nconc dvs (cdr fcs-lst))) + (sort fcs-lst #'(lambda (x y) (>= (length x) (length y)))) + ) + ) + fcs-lst)) + +(defun make-faceset-from-faces (fcs) ;; vertex, edge, normal -> downsizing by remove duplicate + (let ((ht-vrtx (make-hash-table :test #'equal)) + (ht-edgs (make-hash-table :test #'equal)) + edges + ht-normal) + ;; check vertices + (dolist (f fcs) + (let ((vtx (send f :vertices))) + (dotimes (i (length vtx)) + (let ((v (elt vtx i))) + (setf (elt (f . vertices) i) + (sethash v ht-vrtx v)) + ))) + ) + ;; check edges + (dolist (aface fcs) + (let (f ee) + (dolist (e (send aface :edges)) + (if (setq f (gethash (cons (e . nvert) (e . pvert)) ht-edgs)) + (progn ;; if edge is in hash + (setq ee + (find e (send f :edges) ;; search same edge in f + :test #'(lambda (e1 e2) + (and (v= (e1 . pvert) (e2 . nvert)) + (v= (e2 . pvert) (e1 . nvert)))))) + (setf (elt (send aface :edges) + (position e (send aface :edges))) ee) ;; e <- ee + (when ht-vrtx + (setq (ee . pvert) (gethash (ee . pvert) ht-vrtx)) + (setq (ee . nvert) (gethash (ee . nvert) ht-vrtx))) + (setq (ee . nface) aface)) + (progn ;; if edge is not in hash + (setf (gethash (cons (e . pvert) (e . nvert)) ht-edgs) aface) + (push e edges)) + ) + ))) + ;; check normals + ;; nothing + (instance faceset :init :faces fcs + :edges edges + :vertices (send ht-vrtx :list-values) + :primitive (list ':cube 100 100 100)) + )) + +(defun stl2eus-large (fname) + (let* ((fcs (stl-make-faces fname)) + (dev-fcs (make-devided-faces-lists fcs))) + (mapcar #'make-faceset-from-faces dev-fcs) + )) + +;; (setq dev-fcs (make-devided-faces-lists (stl-make-faces "/home/leus/2nd-spine.stl")) a nil) +;; (mapcar #'(lambda (x) (length x)) dev-fcs) + + +(provide :irtcad "$Id: ") diff --git a/irteus/irtwrl.l b/irteus/irtwrl.l new file mode 100644 index 000000000..85af8504b --- /dev/null +++ b/irteus/irtwrl.l @@ -0,0 +1,120 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; $Id: $ +;;; +;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +;;; +;;; This software is a collection of EusLisp code for robot applications, +;;; which has been developed by the JSK Laboratory for the IRT project. +;;; For more information on EusLisp and its application to the robotics, +;;; please refer to the following papers. +;;; +;;; Toshihiro Matsui +;;; Multithread object-oriented language euslisp for parallel and +;;; asynchronous programming in robotics +;;; Workshop on Concurrent Object-based Systems, +;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +;;; +;;; Permission to use this software for educational, research +;;; and non-profit purposes, without fee, and without a written +;;; agreement is hereby granted to all researchers working on +;;; the IRT project at the University of Tokyo, provided that the +;;; above copyright notice remains intact. +;;; + +(in-package "GEOMETRY") + +(export '(eus2wrl)) + +(defun eus2wrl (fname obj &rest args) + (let (set-name) + (unless (send obj :name) + (setq set-name t) + (send obj :name "object")) + (with-open-file (f fname :direction :output) + (apply #'geo::dump-object-to-wrl obj f args)) + (if set-name + (send obj :name nil)))) +;; +;; wrl +;; +(defun dump-object-to-wrl (obj strm &key ((:scale sc) 0.001)) + (let* ((pos (scale sc (send obj :pos))) + (rot (rotation-angle (send obj :rot))) + (children (send obj :descendants)) + (color (get obj :face-color)) + name material diffuse fs vs vlist v rot-v rot-a) + (if (get obj :abbrev-name) (setq name (get obj :abbrev-name)) + (if (send obj :name)(setq name (send obj :name)) + (return-from dump-object-to-wrl nil))) + (if (or (null rot) (memq *nan* (coerce (cadr rot) cons))) + (setq rot-v (float-vector 0 0 1) rot-a 0) + (setq rot-v (cadr rot) rot-a (car rot))) + (format strm "DEF ~A Transform {~%" name) + (format strm " translation ~A ~A ~A~%" + (elt pos 0) (elt pos 1) (elt pos 2)) + (format strm " rotation ~A ~A ~A ~A~%" + (elt rot-v 0) (elt rot-v 1) (elt rot-v 2) rot-a) + (format strm " children [~%") + (when (or (derivedp obj body) (derivedp obj user::bodyset-link)) + (when (derivedp obj body) + (setq fs (send (body-to-faces obj) :faces) + vs (body-model-vertices obj) + vlist vs)) + (when (derivedp obj user::bodyset-link) + (setq fs (send (body-to-faces obj) :faces) + vs (flatten (mapcar #'(lambda (o) (geo::faceset-model-vertices o)) (send obj :bodies))) + vlist vs)) + (format strm " Shape {~%") + (format strm " appearance Appearance {~%") + (when (and color (find-package "GL")) + (setq material (gl::find-color color) + diffuse (send material :diffuse)) + (format strm " material Material {~%") + (format strm " diffuseColor ~A ~A ~A~%" + (elt diffuse 0) (elt diffuse 1) (elt diffuse 2)) + (format strm " }~%") + ) + (format strm " }~%") + (format strm " geometry DEF ~A-FACES IndexedFaceSet {~%" name) + (format strm " ccw TRUE~%") + (format strm " solid TRUE~%") + (format strm " coord DEF ~A-COORD Coordinate {~%" name) + (format strm " point[~%") + (while vlist + (setq v (scale sc (pop vlist))) + (if vlist + (format strm " ~A ~A ~A,~%" + (elt v 0) (elt v 1) (elt v 2)) + (format strm " ~A ~A ~A~%" + (elt v 0) (elt v 1) (elt v 2)) + ) + ) + (format strm " ]~%") + (format strm " }~%") + (format strm " coordIndex [~%") + (while fs + (format strm " ") + (dolist + (p (cdr (send (pop fs) :vertices))) + (format strm "~A, " + (position (send obj :inverse-transform-vector p) vs + :test #'eps-v=)) + ) + (format strm "-1,~%") + ) + (format strm " ]~%") + (format strm " }~%") + (format strm " }~%") + ) + (while children + (if (and (dump-object-to-wrl (pop children) strm :scale sc) + (< 1 (length children))) + (format strm " ,~%")) + ) + (format strm " ]~%") ;; children[ + (format strm "}~%") ;; DEF + t)) + + +(provide :irtcad "$Id: ") diff --git a/irteus/test/test-cad.l b/irteus/test/test-cad.l new file mode 100644 index 000000000..3047cfc98 --- /dev/null +++ b/irteus/test/test-cad.l @@ -0,0 +1,66 @@ +;; test code for euscad + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; stl +(deftest read-write-stl + (let* ((b0 (make-cube 100 100 100)) + (tmpdir (format nil "/tmp/tmp~A" (unix::getpid))) + (tmpstl (format nil "~A/cube.stl" tmpdir))) + (unix::mkdir tmpdir) + (warning-message 2 "writing to ~A~%" tmpstl) + (eus2stl tmpstl (body-to-faces b0)) + ;; + (setq b1 (stl2eus tmpstl)) + + (assert (eps-v= (v- (send (send b1 :box) :maxpoint) (send (send b1 :box) :minpoint)) #f(100 100 100) 3) "check cube size") + )) + +;; wrl +(deftest read-write-wrl + (let* ((b0 (make-cube 100 100 100)) + (tmpdir (format nil "/tmp/tmp~A" (unix::getpid))) + (tmpwrl (format nil "~A/cube.wrl" tmpdir))) + (unix::mkdir tmpdir) + (warning-message 2 "writing to ~A~%" tmpwrl) + (eus2wrl tmpwrl b0) + )) + +;; with robot model +(load "models/h7-robot.l") +(deftest read-write-robot-stl + (let* ((robot (h7)) + (tmpdir (format nil "/tmp/tmp~A" (unix::getpid))) + (tmpstl (format nil "~A/robot.stl" tmpdir)) + b bb1 bb2) + (unix::mkdir tmpdir) + (eus2stl tmpstl robot :scale 0.001) + ;; + (warning-message 2 "writing to ~A~%" tmpstl) + (setq b (stl2eus tmpstl :scale 1000.0)) + + (setq bb1 (make-bounding-box (flatten (send-all (send robot :bodies) :vertices)))) + (setq bb2 (send b :box)) + + (warning-message 2 "original bounding box ~A~%" (v- (send (send bb1 :box) :maxpoint) (send (send bb1 :box) :minpoint))) + (warning-message 2 " stl bounding box ~A~%" (v- (send (send bb2 :box) :maxpoint) (send (send bb2 :box) :minpoint))) + (assert (eps-v= (v- (send (send bb1 :box) :maxpoint) (send (send bb1 :box) :minpoint)) + (v- (send (send bb2 :box) :maxpoint) (send (send bb2 :box) :minpoint)) + 10)) + ) + ) + +(deftest read-write-robot-wrl + (let* ((robot (h7)) + (tmpdir (format nil "/tmp/tmp~A" (unix::getpid))) + (tmpwrl (format nil "~A/robot.wrl" tmpdir)) + b bb1 bb2) + (unix::mkdir tmpdir) + (warning-message 2 "writing to ~A~%" tmpwrl) + (eus2wrl tmpwrl robot) + ) + ) + +(run-all-tests) +(exit) From 177b7e4fac93c0db24cc6e177d9f3b9b36dd916c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 24 Jul 2017 21:45:17 +0900 Subject: [PATCH 2/3] run triangulation https://github.com/jsk-ros-pkg/jsk_model_tools/pull/208#issuecomment-316548668 --- irteus/irtstl.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/irteus/irtstl.l b/irteus/irtstl.l index edec87592..55dc11a13 100644 --- a/irteus/irtstl.l +++ b/irteus/irtstl.l @@ -213,7 +213,7 @@ (defun eus2stl (fname _faceset &key (scale 0.001)) (send _faceset :worldpos) (with-open-file (f fname :direction :output) - (let ((faces (send _faceset :faces))) + (let ((faces (send (body-to-faces _faceset) :faces))) (write-stl-header f (length faces)) (dolist (_face faces) (write-stl-face f _face :scale scale)))) From 9b8f2e69f500e3583eb36d2365a80ab27c430076 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 24 Jul 2017 22:28:24 +0900 Subject: [PATCH 3/3] fix eus2stl, check if the body has glvertices, see https://github.com/jsk-ros-pkg/jsk_model_tools/pull/208#issuecomment-316715445 --- irteus/irtstl.l | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/irteus/irtstl.l b/irteus/irtstl.l index 55dc11a13..5453d9b1d 100644 --- a/irteus/irtstl.l +++ b/irteus/irtstl.l @@ -213,7 +213,21 @@ (defun eus2stl (fname _faceset &key (scale 0.001)) (send _faceset :worldpos) (with-open-file (f fname :direction :output) - (let ((faces (send (body-to-faces _faceset) :faces))) + (let ((faces)) + (cond ((find-method _faceset :bodies) + (setq faces + (mapcan #'(lambda (x) + ;(print (list x (boundp 'gl::glbody) (classp gl::glbody) (derivedp x gl::glbody))) + (cond ((find-method x :glvertices) + (send (body-to-faces (send x :glvertices)) :faces)) + ((cdr (assoc 'user::glvertices (send x :slots))) + (send (cdr (assoc 'user::glvertices (send x :slots))) :faces)) + (t + (send (body-to-faces x) :faces)) + )) + (send _faceset :bodies)))) + (t + (setq faces (send (body-to-faces _faceset) :faces)))) (write-stl-header f (length faces)) (dolist (_face faces) (write-stl-face f _face :scale scale))))