ハノイの塔を二本のアームを使って解くサンプル

hanoi-arm.l

(load "sample-arm-model.l")
(load "hanoi.l")

(defmethod cascaded-link
  (:pick
   (obj &rest args)
   (send self :open-hand)
   (prog1
       (send* self :solve-ik obj args)
     (send self :close-hand obj))
   )
  (:place
   (obj &rest args)
   (prog1
       (send* self :solve-ik obj args)
     (send self :open-hand)))
  )

(defun decide-arm (from to)
  (if (eq to *table-c*)
    *sarm1*
    (if (eq to *table-a*)
      *sarm2*
      (if (eq from *table-c*)
        *sarm1*
        *sarm2*))))

(defun avoid-collision (arm)
  (if (eq arm *sarm1*)
    (send *sarm1* :place (make-cascoords :pos #f(300 -380 450)) :rotation-axis :z :debug-view :no-message)
    (send *sarm2* :place (make-cascoords :pos #f(300 380 450)) :rotation-axis :z :debug-view :no-message)
    ))

(defun move-disk (d from to) ;; re-define
  (let (pav)
    (setq *sarm* (decide-arm from to))
    (setq *sarmwait* (if (eq *sarm* *sarm1*) *sarm2* *sarm1*))
    (avoid-collision *sarmwait*)
    (if (eq from 'ground)
        (move-on d (table-top to))
      (progn
        (move-arm d 'pick)
        (move-arm (get (table-top to) :top-coords) 'place)))
    (setf (get from :disks) (cdr (get from :disks)))
    (setf (get to :disks) (cons d (get to :disks)))
    ))

(defun move-arm (to pick/place &optional (count 3))
  (let ()
    (unless
        (send *sarm* (if (eq pick/place 'pick) :pick :place)
              to :rotation-axis :z
              :obstacles
              (flatten (list (remove-if #'(lambda (x) (< (norm (send x :difference-position to :translation-axis :z)) 50)) *tables*) *sarmwait*))
              :stop 500
;; collision free motion generation stucks...
              :avoid-collision-distance   200 ;; 200
              :avoid-collision-joint-gain 2.0 ;; 1.0
              :avoid-collision-null-gain  1.0 ;; 1.0
              :debug-view :no-message
              )
        (send *sarmwait* :place
               (send (send (send (send *sarmwait* :end-coords) :worldcoords ) :copy-coords)
                     :translate (float-vector (if (eq *sarmwait* *sarm1*) -50 50) 0 0))

              :rotation-axis :z
              :obstacles
              (flatten (list (remove-if #'(lambda (x) (< (norm (send x :difference-position to :translation-axis :z)) 50)) *tables*) *sarm*))
              :stop 500
;; collision free motion generation stucks...
              :avoid-collision-distance   200 ;; 200
              :avoid-collision-joint-gain 2.0 ;; 1.0
              :avoid-collision-null-gain  1.0 ;; 1.0
              :debug-view :no-message
              )
      (if (> count 0)
        (move-arm to pick/place (- count 1))
        (break))
      )
    (send *irtviewer* :draw-objects)))

;; sarm sample
(defun hanoi-arm (&optional (discnum 3))
  (send *irtviewer* :title "hanoi-arm")
  (setq *sarm1* (instance sarmclass :init))
  (setq *sarm2* (instance sarmclass :init))
  (send *sarm1* :locate #f(0 -100 0))
  (send *sarm2* :locate #f(0 100 0))
  (send *sarm1* :reset-pose)
  (send *sarm2* :reset-pose)
  (objects (list *sarm1* *sarm2*))

  (send *sarm1* :reset-pose)
  (send *sarm2* :reset-pose)
  (send *irtviewer* :draw-objects)
  ;(send *sarm1* :solve-ik (make-cascoords :pos #f(500 0 100))
  ;      :avoid-collision-null-gain  0.0 ;; 1.0
  ;      :rotation-axis t :debug-view nil)
  (send *sarm1* :open-hand)
  (send *sarm2* :open-hand)
  (send *irtviewer* :draw-objects)

  (setq *table-a* (hanoi-table :name "table-a" :height 320 :pos #f(300 200 0)))
  (setq *table-b* (hanoi-table :name "table-b" :height 350 :pos #f(350 0 0)))
  (setq *table-c* (hanoi-table :name "table-b" :height 300 :pos #f(300 -200 0)))
  (setq *tables* (list *table-a* *table-b* *table-c*))
  (hanoi-init discnum *table-a*)
  (setq *disk-1* (car *disks*))
  (setq *disk-2* (cadr *disks*))
  (setq *disk-3* (caddr *disks*))
  (send *sarm1* :reset-pose)
  (send *sarm2* :reset-pose)
  (objects (flatten (list  *tables* *disks* *sarm1* *sarm2*)))

  (dolist (action (hanoi-program (length *disks*)))
    (print action)
    (eval action)
    (unix:usleep (* 100 1000))
    ))

(unless (boundp '*irtviewer*) (make-irtviewer))
(warn "(hanoi-arm) for arm solving hanoi tower~%")