マニピュレータ

documented by Hiromu Onda

rotational-jointクラスとmanipulatorクラスのインスタンスからマニピュレータ モデルは構成される。rotational-joinクラスは、bodyのサブクラスであり、 マニピュレータの間接モデルを定義する。 manipulatorは、cascaded-coordsのサブクラスであり、運動方程式と逆運動方程式の 解を求めるメソッドを持っている。

マニピュレータを定義するには、 すべての関節を作成した後、manipulatorにそれらを統合する。

関節のモデル

クラスrotational-jointが関節のモデルを記述する。 クラスrotational-jointは、bodyをスーパクラスに持ち、 その形状モデル、座標系に加えて 関節回転軸、回転角度、可動角度範囲、などを管理している。 次のdefjointマクロによってrotational-joint のインスタンスが作成され、joint-nameにバインドされる。 parentには、親の関節を指定する。 ベースと指には可動軸を指定する必要はない。

defjoint   [マクロ] 

joint-name &key :shape  body-object 

:color 		 color-id   ;0-15 for MMD 

:parent 		 parent-joint 

:axis 		 rotational-axis 		  ; :x, :y or :z 

:offset 		 trans-from-parent-joint 

:low-limit 		 joint-angle-limit-low 

:high-limit 		 joint-angle-limit-hight

関節のモデルを記述する。

多関節マニピュレータ

マニピュレータモデルはクラスmanipulatorによって記述される。 マニピュレータモデルを作成するには、 次のdefmanipulatorマクロを用いる。

defmanipulator   [マクロ] 

manipulator-name &key 

:class   manipulator-class 

:base 		 base-joint 

:joints 		  list-of-all-joints 

:hand 		 handjoint 

:left-finger 		 left-finger

:right-finger 		 right-finger

:handcoords 		 trans-from-hand-to-armsolcoords

:toolcoords 		 trans-from-armsolcoords-to-toolcoords 

:open-direction 		 finger-open-direction

:right-handed 		 righty-or-lefty

マニピュレータモデルを作成する。

rotational-joint [クラス]

  :super   body 

  :slots 		 (axis offset high-limit low-limit) 

6自由度マニピュレータの間接を記述する。

manipulator [クラス]

  :super   cascaded-coords 

  :slots 		 (base baseinverse joint


angles right-handed hand handcoords right-finger left-finger


openvec max-span toolcoords toolinverse armsolcoords


toolinverse armsocoords approach grasp affix) 

ベースからハンドまでのマニピュレータの運動を管理する。

:newcoords newrot &optional newpos [メソッド]

関節角度が限度に収まっていれば座標系を newrotとnewposに更新する。

:armsolcoords [メソッド]

ベース座標系からハンド座標系への変換（座標系のインスタンス）を計算し、作成する。

:tool &rest msg [メソッド]

工具座標系を返す、または変更する。

:set-tool newtool &optional offset copy [メソッド]

工具座標系toolcoordsにnewtoolを設定する。

:reset-tool [メソッド]

工具座標系を初期値に戻す。

:worldcoords [メソッド]

工具座標系の位置ベクトル、回転行列、座標系のワールド表現を求める。

:set-coords [メソッド]

順運動の解を求めるために、座標系を強制的に設定する。

:config &optional (a newangles) [メソッド]

6つの関節角度を直接に設定する。

:park [メソッド]

初期姿勢に戻す。

:hand &optional (h nil) [メソッド]

ハンドオブジェクトを返す。

:handcoords [メソッド]

ハンド座標系の位置ベクトル、回転行列、座標系のワールド表現を求める。

:span [メソッド]

現在の指の間隔を返す。

:open-fingers s &optional abs &aux (current (send self :span)) [メソッド]

指幅を相対的、絶対的に指定する。

:close-fingers [メソッド]

指を完全に閉じる。

:angles &optional flag [メソッド]

現在の姿勢の関節角度のリストを返す。

:get-approach [メソッド]

現在アプローチしている対象を返す。

:set-approach a [メソッド]

アプローチ対象aを設定する。

:get-grasp [メソッド]

(:get-grasp () grasp-config)

:set-grasp g [メソッド]

把握対象物gを指定する。

:get-affix [メソッド]

把握している物体を返す。

:affix &optional (grasp) [メソッド]

affixed-objectにgraspを設定する。 graspは、子孫としてhandcoordsに関連付けられる。

:unfix &optional (margin 10.0) [メソッド]

affixed-objectにNILを設定する。 graspは、handcoordsの子孫リストから外される。

:create   [メソッド] 

&rest args 

&key (name nm) (hand h) (joints j)

(left-finger lf) (right-finger rf)

((toolcoords tc) (make-coords))

((handcoords hc) (make-coords))

((base bs) (make-cascoords))

(open-direction (floatvector 0 1 0))

((max-span mspan) 100.0)

((lefty lft) t)

((act a) nil)

&allow-other-keys

新しいマニピュレータオブジェクトを作成、初期化する。

manipulatorオブジェクトは、 base、joints(J1...J6)、handcoords、toolcoords の座標系の繋がりを管理する。 manipulatorクラスは、cascaded-coordsのサブクラスであり、 やはり、cascaded-coords(またはbodyなどのサブクラス) であるbaseに結合され、 baseからtoolcoords(手先座標系)への変換を管理している。 したがって、manipulatorオブジェクトに対して送られる :translate、:locate、:rotate、:orient、:transform などのメッセージは、手先点に対して作用する。 そのとき同時にWRTパラメータを指定すれば、 手先はWRT座標系に対して動く。 次のプログラムでは、eta3をmanipulatorのインスタンスと仮定している。

　(send eta3 :translate #f(0 0 -100))        ;手先を10cm引っ込める　
　(send eta3 :translate #f(0 0 -100) :world) ;10cm下げる
　(send eta3 :translate #f(0 0 -100)
             (manipulator-base eta3))     ;手先をベース座標系で10cm下げる

これらのメッセージに対して、manipulatorはアーム解を計算して6つの 関節角度を決定する。 一般に解は複数存在するが、right-handed(右手系、左手系) の区別、および現在の関節角度との連続性により適当な解が選択される。 しかし、指定された位置、姿勢に対する解が存在しない場合や関節角が 限界を越える場合は移動、回転は起こらず、警告が発せられる。

アーム解の計算は、実際のマニピュレータに対応した 個々のmanipulatorクラスに定義された:armsolメソッドが行う。 マニピュレータがワールド座標系のどこに置かれてもよいように、 また、どのような工具を用いてもよいように、アーム解は、 base、toolcoordsとは独立に、base座標系中でのハンドの位置、姿勢に 対して与えられる。

base、J1、J2、...、handcoords、toolcoordsの関係を図20 に示す。 ワールドから手先への変換を$T$とすると、$T$および各部分変換は次のようにし て得られる。

$$\begin{array}{ll} T & = base \cdot J1 \cdot J2 \cdot \ldots ... ...coords \\ & = (manipulator-handcoords \; eta3)\\ \end{array}$$

ここで、$T$はワールド座標系から工具座標系まで変換する。

Figure 20: relation between coordinate systems in a manipulator

各関節は、Brepで表現された幾何モデルを保持している。しかし、頂点の座標、 平面の方程式は常に現状を反映しているとは限らない。マニピュレータに対する 移動、回転などのメッセージでは座標系の更新だけを行い、頂点の座標は変化し ない。これは、移動、回転が複数回続けて起こった場合の計算量を減らすためで ある。更新は、マニピュレータに:worldcoordsメッセージを送る ことで引き起こされる。

マニピュレータは、手先座標系で動作を指定することを主な目的としている。 関節角による指定には :config を用いる。 引き数には6要素の列を与える。

　 (send eta3 :config (float-vector pi/2 pi/2 0 1 0 1))

:configは、各関節角度が可動範囲に収まっていることを検査した後、 それらを回転させる。 この結果、マニピュレータの管理している座標系と 関節角度から定まる実際の手先の位置姿勢とが一致しなくなる。 両者を一致させるためには、:set-coordsメッセージを送る。 :set-coordsは、関節角度から順方向のキネマティクスを計算し、 最終的な手先座標系に対してさらにアーム解を解く。

例 ETA3のモデル生成とその描画

EusLisp 7.27 with Xlib created on Thu Sep 17 14:33:30 1992
(load "view.l")                                ;ウィンドウを開く
(load "/usr/local/eus/robot/eta3/eta3build.l") ;ETA3のモデルを生成する
(send *viewing* :look #f(2000 2000 2000))      ;視点を変える
(send-all (eta3arm-components eta3) :color 1)  ;物体の線の色を黒に変える
(send eta3 :config (float-vector 0 (/ -Pi 4.0) Pi/2 0 (/ -Pi 4.0) 0 ))
					       ;ETA3を関節角度の指定で動かす
(send eta3 :set-coords)                        ;上記参照
(draw eta3)                                    ;ETA3を描画する