実際のロボットモデル

実際のロボットや環境を利用した実践的なサンプルプログラムを見てみよう．

まず，最初はロボットや環境のモデルファイルを読み込む．これらのファイル は$EUSDIR/modelsに，これらのファイルをロードしインスタンスを生成す るプログラムは以下のように書くことができる．(room73b2)や (h7)はこれらのファイル内で定義されている関数である． ロボットのモデル(robot-model)はirtrobot.lファイルで定義 されており，cascaded-linkクラスの子クラスになっている． ロボットとはlarm,rarm,lleg,rleg,headのリンクのツリーからなる ものとして定義されており， (send *robot* :larm)や(send *robot* :head)として ロボットのリム(limb)にアクセスでき，右手の逆運動学，左手の逆運動学等と いう利用方法が可能になっている．

(load "models/room73b2-scene.l")
(load "models/h7-robot.l")
(setq *room* (room73b2))
(setq *robot* (h7))
(objects (list *robot* *room*))

ロボットには:reset-poseというメソッドがありこれで初期姿勢をとる ことができる．

(send *robot* :reset-pose)

次に，ロボットを部屋の中で移動させたい．部屋内の代表的な座標は (send *room* :spots)で取得できる．この中から目的の座標を得る 場合はその座標の名前を引数として:spotメソッドを呼び出す． ちなみに，このメソッドの定義はprog/jskeus/irteus/irtscene.l にあり

(defmethod scene-model
  (:spots
   (&optional name)
   (append
    (mapcan
     #'(lambda(x)(if (derivedp x scene-model) (send x :spots name) nil))
     objs)
    (mapcan #'(lambda (o)
		(if (and (eq (class o) cascaded-coords)
			 (or (null name) (string= name (send o :name))))
		    (list o)))
	    objs)))
  (:spot
   (name)
   (let ((r (send self :spots name)))
     (case (length r)
       (0 (warning-message 1 "could not found spot(~A)" name) nil)
       (1 (car r))
       (t (warning-message 1 "found multiple spot ~A for given name(~A)" r name) (car r)))))
  )

となっている．

ロボットもまたcoordinatesクラスの子クラスなので:move-to メソッドを利用できる．また，このロボットの原点は腰にあるので足が地面に つくように:locateメソッドを使って移動する．

(send *robot* :move-to (send *room* :spot "cook-spot") :world)
(send *robot* :locate #f(0 0 550))

現状では*irtviewer*の画面上でロボットが小さくなっているので， 以下のメソッド利用し，ロボットが画面いっぱいになるように調整する．

(send *irtviewer* :look-all
      (geo::make-bounding-box
       (flatten (send-all (send *robot* :bodies) :vertices))))

次に環境中の物体を選択する．ここでは:objectメソッドを利用する． これは，:spots, :spotと同様の振る舞いをするため， どのような物体があるかは，(send-all (send *room* :objects) :name) として知ることができる． room73b2-kettleの他に room73b2-mug-cupやroom73b2-knife等を利用するとよい．

(setq *kettle* (send *room* :object "room73b2-kettle"))

環境モデルの初期化直後は物体は部屋にassocされているため，以下の用に 親子関係を解消しておく．こうしないと物体を把持するなどの場合に問題が生 じる．

(if (send *kettle* :parent) (send (send *kettle* :parent) :dissoc *kettle*))

ロボットの視線を対象物に向けるためのメソッドとして以下のようなものがあ る．

(send *robot* :head :look-at (send *kettle* :worldpos))

対象物体には，その物体を把持するための利用したらよい座標系が :handleメソッドとして記述されている場合がある．このメソッドは リストを返すため以下の様に(car (send *kettle* :handle))として その座標系を知ることができる．この座標がどこにあるか確認するためには (send (car (send *kettle* :handle)) :draw-on :flush t)とすると よい．

したがってこの物体手を伸ばすためには

(send *robot* :larm :inverse-kinematics
      (car (send *kettle* :handle))
      :link-list (send *robot* :link-list (send *robot* :larm :end-coords :parent))
      :move-target (send *robot* :larm :end-coords)
      :rotation-axis :z
      :debug-view t)

となる．

ここで，ロボットの手先と対象物体の座標系を連結し，

(send *robot* :larm :end-coords :assoc *kettle*)

以下の様にして世界座標系で100[mm]持ち上げることができる．

(send *robot* :larm :move-end-pos #f(0 0 100) :world
        :debug-view t :look-at-target t)

:look-at-targetは移動中に首の向きを常に対象を見つづけるようにす るという指令である．