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

irteusではロボットリンクを記述するクラスとしてbodyset-link(irtmodel.l) というクラスが用意されている．これは機構情報と幾何情報をもち，一般的な木 構造でロボットの構造が表現されている．また，jointクラスを用いて関節情報 を扱っている．

(defclass bodyset-link
  :super bodyset
  :slots (joint parent-link child-links analysis-level default-coords
                weight acentroid inertia-tensor
                angular-velocity angular-acceleration
                spacial-velocity spacial-acceleration
                momentum-velocity angular-momentum-velocity
                momentum angular-momentum
                force moment ext-force ext-moment))

ジョイント（関節）のモデリングはjointクラス(irtmodel.l)を用いる．jointクラスは基底ク ラスであり，実際にはrotational-joint, linear-joint等を利用する． jointの子クラスで作られた関節は，:joint-angleメソッドで関節角度を指定す ることが出来る．

(defclass joint
  :super propertied-object
  :slots (parent-link child-link joint-angle min-angle max-angle
	default-coords))
(defmethod joint
  (:init (&key name
               ((:child-link clink)) ((:parent-link plink))
               (min -90) (max 90) &allow-other-keys)
         (send self :name name)
         (setq parent-link plink child-link clink
               min-angle min max-angle max)
         self))

(defclass rotational-joint
  :super joint
  :slots (axis))
(defmethod rotational-joint
  (:init (&rest args &key ((:axis ax) :z) &allow-other-keys)
         (setq axis ax joint-angle 0.0)
         (send-super* :init args)
         self)
  (:joint-angle
   (&optional v)
   (when v
        (setq relang (- v joint-angle) joint-angle v)
        (send child-link :rotate (deg2rad relang) axis)))
     joint-angle))

ここでは，joint, parent-link, child-links, defualt-coordsを利用する．

簡単な１関節ロボットの例としてサーボモジュールを作ってみると

(defun make-servo nil
  (let (b1 b2)
    (setq b1 (make-cube 35 20 46))
    (send b1 :locate #f(9.5 0 0))
    (setq b2 (make-cylinder 3 60))
    (send b2 :locate #f(0 0 -30))
    (setq b1 (body+ b1 b2))
    (send b1 :set-color :gray20)
    b1))

(defun make-hinji nil
  (let ((b2 (make-cube 22 16 58))
        (b1 (make-cube 26 20 54)))
    (send b2 :locate #f(-4 0 0))
    (setq b2 (body- b2 b1))
    (send b1 :set-color :gray80)
    b2))

(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 r (instance cascaded-link :init))
(send r :assoc h1)
(send h1 :assoc s1)
(setq (r . links) (list h1 s1))
(setq (r . joint-list) (list j1))
(send r :init-ending)

となる．

ここでは，h1，s1というbodyset-linkと， j1というrotational-jointを作成し，ここから cascaded-linkという，連結リンクからなるモデルを生成している． cascaded-linkはcascaded-coordsの子クラスであるため， r (cascaded-link)，h1，s1の座標系の親子関係を :assocを利用して設定している．

(r . links)という記法はrというオブジェクトのスロット変数 （メンバ変数）であるlinksにアクセスしている．ここでは， linksおよびjoint-listに適切な値をセットし， (send r :init-ending)として必要な初期設定を行っている．

これでrという2つのリンクと１つの関節情報 を含んだ1つのオブジェクトを生成できる．これで例えば (objects (list h1 s1))ではなく， (objects (list r))としてロボットをビューワに表示できる． また，(send r :locate #f(100 0 0))などの利用も可能になっている．

cascaded-linkクラスのメソッドの利用例としては以下ある． :joint-list，:linksといった関節リストやリンクリストへの アクセサに加え，関節角度ベクトルへのアクセスを提供する :angle-vectorメソッドが重要である．これを引数なしで呼び出せば現 在の関節角度が得られ，これに関節角度ベクトルを引数に与えて呼び出せば，その引 数が示す関節角度ベクトルをロボットモデルに反映させることができる．

$ (objects (list r))
(#<servo-model #X628abb0  0.0 0.0 0.0 / 0.0 0.0 0.0>)
;; useful cascaded-link methods
$ (send r :joint-list)
(#<rotational-joint #X6062990 :joint101067152>)
$ (send r :links)
(#<bodyset-link #X62ccb10 :bodyset103598864  0.0 0.0 0.0 / 0.0 0.0 0.0>
 #<bodyset-link #X6305830 :bodyset103831600  0.0 0.0 0.0 / 0.524 0.0 0.0>)
$ (send r :angle-vector)
#f(0.0)
$ (send r :angle-vector (float-vector 30))
#f(30.0)