background.l

(if (not (boundp '*irtviewer*))
    (make-irtviewer))
(require "models/akira-robot.l")
(require "models/human-robot.l")
(require "/opt/ros/melodic/share/pr2eus/pr2.l")
(setq robot-fn #'akira)
;; (setq robot-fn #'human)
;; (setq robot-fn #'pr2)

;; write white  back
(setq *robot* (funcall robot-fn))
(objects (list *robot*))
(send *irtviewer* :change-background #f(1 1 1))
(send *irtviewer* :draw-objects)
(send *irtviewer* :save-image "test.png" :background #f(1 1 1))

;; write hid mode
(setq *robot* (funcall robot-fn))
(send *irtviewer* :change-background #f(0 0 0))
(send *irtviewer* :select-drawmode t)    ;; force reset viwer mode
(objects (list *robot*))
(send *irtviewer* :select-drawmode 'hid)
(objects (list *robot*))
(send *irtviewer* :save-image "test-hid.png" :background #f(0 0 0))

;;
(send *irtviewer* :select-drawmode t)
(objects (list *robot*))

(defun make-bone-objects (robot &key cyl-l cyl-r bon-s)
  (let (analysis-robot
    c l b
    (bb (make-bounding-box (flatten (send-all (send robot :bodies) :vertices)))))
    
    (if (null cyl-l)
    (setq cyl-l (/ (elt (v- (send bb :maxpoint) (send bb :minpoint)) 0) 15)))
    (if (null cyl-r)
    (setq cyl-r (* cyl-l 4)))
    (if (null bon-s)
    (setq bon-s (/ (elt (v- (send bb :maxpoint) (send bb :minpoint)) 2) 100)))
    (setq analysis-robot nil)
    (dolist (j (send *robot* :joint-list))
      (setq c (make-cylinder cyl-l cyl-r))
      (send c :translate-vertices (float-vector 0 0 (/ cyl-r -2)))
      ;; bug!! https://github.com/euslisp/EusLisp/issues/499
      (cond ((memq (j . axis) '(:x :y :z))
             (send c :rotate-vertices pi/2 (j . axis)))
            ((eq (j . axis) :-x)
             (send c :rotate-vertices pi/2 :x))
            ((eq (j . axis) :-y)
             (send c :rotate-vertices pi/2 :y))
            ((eq (j . axis) :-z)
             (send c :rotate-vertices pi/2 :z))
            ((eps= (abs (v. (j . axis) #f(0 0 1))) 1.0)
             (send c :rotate-vertices pi/2 :z))
            ((eps= (abs (v. (j . axis) #f(0 1 0))) 1.0)
             (send c :rotate-vertices pi/2 :x))
            ((eps= (abs (v. (j . axis) #f(1 0 0))) 1.0)
             (send c :rotate-vertices pi/2 :y)))
      (send c :newcoords (send (send j :child-link) :copy-worldcoords))
      (push c analysis-robot)
      (setq l (v- (send (send j :child-link) :worldpos)
          (send (send j :parent-link) :worldpos)))
      (when (eps> (norm l) 0)
    (setq b (make-cube bon-s (* bon-s 2) (norm l)))
    (send b :translate-vertices (float-vector 0 0 (/ (norm l) -2)))
    (send b :locate (send (send j :child-link) :worldpos))
    (send b :newcoords (orient-coords-to-axis b (normalize-vector l)))
    (push b analysis-robot))
      )
    (dolist (e (robot . end-coords-list))
      (setq l (v- (send e :worldpos)
                  (send (send e :parent) :worldpos)))
      (when (eps> (norm l) 0)
        (setq b (make-cube (* bon-s 2) (* bon-s 2) (norm l)))
        (send b :translate-vertices (float-vector 0 0 (/ (norm l) -2)))
        (send b :locate (send e :worldpos))
        (send b :newcoords (orient-coords-to-axis b (normalize-vector l)))
        (push b analysis-robot)))
      analysis-robot))

  ;;(objects (flatten (list *robot* analysis-robot)))
(send *irtviewer* :change-background #f(0 1 0))
(objects (make-bone-objects *robot*))

(send *irtviewer* :save-image "test-bone.png" :background #f(0 1 0))