drc-irtmodel-to-urdf.l

#!/usr/bin/env irteusgl
;; drc-irtmodel-to-urdf.l
;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>

(defun rospack-find (pkg)
  (string-downcase (read (piped-fork (format nil "rospack find ~A" pkg)))))

(load (format nil "~A/euslisp/hrpsys-gazebo-utils.l" (rospack-find "hrpsys_gazebo_tutorials")))

(load "drc-stair-object.l")
(load "drc-testbed-drill-wall-object.l")
(load "drc-testbed-rope-object.l")
(load "drc-terrain-object.l")
(load "drc-testbed-hose-plug-object.l")
(load "drc-testbed-shower-object.l")
(load "drc-testbed-button-object.l")
(load "drc-testbed-hose-wall-object.l")
(load "drc-testbed-valve-object.l")
(load "drc-testbed-door-object.l")
(load "drc-testbed-lever-object.l")

(defun to-camel (s)
  (concatenate string
               (string-upcase (subseq s 0 1))
               (subseq s 1 (length s))))

(defun split-string (d s)
  (let ((ptr 0)
        (pptr 0)
        ret)
    (while (< ptr (length s))
      (when (string= d (subseq s ptr (+ ptr (length d))))
        (push (subseq s pptr ptr) ret)
        (incf ptr (length d))
        (setq pptr ptr))
      (incf ptr))
    (push (subseq s pptr ptr) ret)
    (reverse ret)))

(defun to-urdf-name (s)
  (apply #'concatenate string
               "DRC"
               (mapcar #'to-camel
                       (cdr (split-string "-" s)))))

(setq *models*
      (list
       (setq *stair* (drc-stair))
       (setq *drill-wall* (drc-testbed-drill-wall))
       (setq *surprise-task-rope* (drc-testbed-rope))
       (setq *terrain* (drc-terrain))
       (setq *hose-plug* (drc-testbed-hose-plug))
       (setq *hose-wall* (drc-testbed-hose-wall))
       (setq *surprise-task-shower* (drc-testbed-shower))
       (setq *surprise-task-button* (drc-testbed-button))
       (setq *drc-valve* (drc-testbed-valve))
       (setq *door* (drc-testbed-door))
       (setq *surprise-task-lever* (drc-testbed-lever))))

(send *stair* :translate (float-vector 8000 0 0) :world)
(send *door* :translate (float-vector 0 0 0) :world)
(send *hose-wall* :rotate (deg2rad -45) :z)
(send *hose-wall* :translate (float-vector 4000 -3000 0) :world)
(send *drill-wall* :rotate -pi/2 :z)
(send *drill-wall* :translate (float-vector 2000 -3500 0) :world)
(send *terrain* :translate (float-vector 5000 -1000 0) :world)
(send *drc-valve* :rotate -pi/2 :y)
(send *drc-valve* :rotate (deg2rad -135) :x)
(send *drc-valve* :translate (float-vector 1211.58 -3432.82 1130.0) :world)

(send *surprise-task-button* :rotate pi/2 :z)
(send *surprise-task-shower* :translate #f(11335 0 0) :world)
(send *surprise-task-button* :translate #f(12000 -665 0) :world)
(send *surprise-task-lever* :rotate pi :z)
(send *surprise-task-lever* :translate #f(12665 0 0) :world)
(send *surprise-task-rope* :rotate -pi/2 :z)
(send *surprise-task-rope* :translate #f(12000 665 0) :world)

(objects *models*)
;;(break)
(setq package-dir-path (string-downcase (read (piped-fork "rospack find hrpsys_gazebo_tutorials"))))
(setq world-src-file-path (format nil "~a/worlds/world_source.world" package-dir-path))
(setq world-new-file-path (format nil "~a/worlds/~a.world" package-dir-path "DRCTestbed"))
(setq world-file-path  world-new-file-path)
(unix:system (format nil "rm -rf ~a" world-new-file-path))
(unix:system (format nil "cp ~a ~a" world-src-file-path world-new-file-path))

(dolist (obj *models*)
  (send obj :name (to-urdf-name (send obj :name)))
  (warn "generating ~A~%" (send obj :name))
  (let* ((pos (scale 0.001 (send obj :worldpos)))
         (obj-name (send obj :name))
         (pos-string (format nil "~a ~a ~a" (elt pos 0) (elt pos 1) (elt pos 2)))
         (rpy (car (send obj :rpy-angle)));;(car (rpy-angle (send obj :worldrot))))
         (rpy-string (format nil "~a ~a ~a" (elt rpy 2) (elt rpy 1) (elt rpy 0)))
         (prev-pos (send obj :copy-worldcoords)))
    (send obj :reset-coords)
    (irteus2urdf-for-gazebo obj (send obj :name))
    (send obj :transform prev-pos)

    (unix:system (format nil "sed -i -e \"s@    <!-- Objects -->@    <!-- Objects -->\\n    <include>\\n      <uri>model://~a_static</uri>\\n      <pose>~a ~a</pose>\\n    </include>@g\" ~a" obj-name pos-string rpy-string world-file-path))
    (unix:system (format nil "rosrun hrpsys_gazebo_tutorials make_static_model.py ~a" obj-name))
    (unix:system (format nil "rosrun hrpsys_gazebo_tutorials make_fixed_model.py ~a" obj-name))
    )
)

(unix:system (format nil "rosrun hrpsys_gazebo_tutorials generate_model_database.py"))