Subsections


マニピュレータ

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 [メソッド]

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


:armsolcoords [メソッド]

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


:tool &rest msg [メソッド]

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


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

工具座標系toolcoordsnewtoolを設定する。


: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-objectgraspを設定する。 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座標系に対して動く。 次のプログラムでは、eta3manipulatorのインスタンスと仮定している。

 (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{displaymath}
\begin{array}{ll}
T & = base \cdot J1 \cdot J2 \cdot \ldots ...
...coords \\
& = (manipulator-handcoords \; eta3)\\
\end{array}\end{displaymath}

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

Figure 20: relation between coordinate systems in a manipulator
\includegraphics[height=100mm]{fig/eta3coords.ps}

各関節は、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を描画する

This document was generated using the LaTeX2HTML translator on Sat Feb 5 14:36:44 JST 2022 from EusLisp version 138fb6ee Merge pull request #482 from k-okada/apply_dfsg_patch