cascaded-linkを用いたロボット（多リンク系）のモデリング

一方で多リンク系のモデリング用のクラスとしてcascaded-linkというクラス がある． これには，links, joint-listというスロット変数があり，ここにbodyset-link, ならびにjointのインスタンスのリストをバインドして利用する． 以下は， cascaded-linkの子クラスを定義しここでロボットモデリングに 関する初期化処理を行うという書き方の例である．

(defclass cascaded-link
  :super cascaded-coords
  :slots (links joint-list bodies collision-avoidance-links))

(defmethod cascaded-link
  (:init (&rest args &key name &allow-other-keys)
         (send-super-lexpr :init args)
         self)
  (:init-ending
   ()
   (setq bodies (flatten (send-all links :bodies)))
   (dolist (j joint-list)
     (send (send j :child-link) :add-joint j)
     (send (send j :child-link) :add-parent-link (send j :parent-link))
     (send (send j :parent-link) :add-child-links (send j :child-link)))
   (send self :update-descendants))
)

(defclass servo-model
  :super cascaded-link
  :slots (h1 s1 j1))
(defmethod servo-model
  (:init ()
   (let ()
     (send-super :init)
     (setq h1 (instance bodyset-link :init (make-cascoords) :bodies (list (make-hinji))))
     (setq s1 (instance bodyset-link :init (make-cascoords) :bodies (list (make-servo))))

     (setq j1 (instance rotational-joint :init :parent-link h1 :child-link s1 :axis :z))

     ;; instance cascaded coords
     (setq links (list h1 s1))
     (setq joint-list (list j1))
     ;;
     (send self :assoc h1)
     (send h1 :assoc s1)
     ;;
     (send self :init-ending)
     self))
  ;;
  ;; (send r :j1 :joint-angle 30)
  (:j1 (&rest args) (forward-message-to j1 args))
  )

(setq r (instance servo-model :init))

このようなクラスを定義して(setq r (instance servo-model :init)) としても同じようにロボットモデルのインスタンスを作成することができ，先 ほどのメソッドを利用できる．クラス定義するメリットは (:j1 (&rest args) (forward-message-to j1 args))というメソッド定 義により，関節モデルのインスタンスへのアクセサを提供することができる． これにより，特定の関節だけの値を知りたいとき，あるいは値をセットしたい 時には(send r :j1 :joint-angle)や (send r :j1 :joint-angle 30) という指示が可能になっている． このロボットを動かす場合は前例と同じように

(objects (list r))
(dotimes (i 300)
  (send r :angle-vector (float-vector (* 90 (sin (/ i 100.0)))))
  (send *irtviewer* :draw-objects))

などとするとよい．

(setq i 0)
(do-until-key
  (send r :angle-vector (float-vector (* 90 (sin (/ i 100.0)))))
  (send *irtviewer* :draw-objects)
  (incf i))

とすると，次にキーボードを押下するまでプログラムは動きつづける．

さらに，少し拡張して これを用いて３リンク２ジョイントのロボットをモデリングした例が以下にな る．:joint-angleというメソッドに#f(0 0)というベクトルを引数に与えるこ とで全ての関節角度列を指定することが出来る．

(defclass hinji-servo-robot
  :super cascaded-link)
(defmethod hinji-servo-robot
  (:init
   ()
   (let (h1 s1 h2 s2 l1 l2 l3)
     (send-super :init)
     (setq h1 (make-hinji))
     (setq s1 (make-servo))
     (setq h2 (make-hinji))
     (setq s2 (make-servo))
     (send h2 :locate #f(42 0 0))
     (send s1 :assoc h2)
     (setq l1 (instance bodyset-link :init (make-cascoords) :bodies (list h1)))
     (setq l2 (instance bodyset-link :init (make-cascoords) :bodies (list s1 h2)))
     (setq l3 (instance bodyset-link :init (make-cascoords) :bodies (list s2)))
     (send l3 :locate #f(42 0 0))

     (send self :assoc l1)
     (send l1 :assoc l2)
     (send l2 :assoc l3)

     (setq joint-list
           (list
            (instance rotational-joint
                      :init :parent-link l1 :child-link l2
                      :axis :z)
            (instance rotational-joint
                      :init :parent-link l2 :child-link l3
                      :axis :z)))
     (setq links (list l1 l2 l3))
     (send self :init-ending)
     )))
(setq r (instance hinji-servo-robot :init))
(objects (list r))

(dotimes (i 10000)
  (send r :angle-vector (float-vector (* 90 (sin (/ i 500.0))) (* 90 (sin (/ i 1000.0)))))
  (send *irtviewer* :draw-objects))