|
1 | 1 | \section{干渉計算} |
2 | 2 |
|
3 | | -干渉計算には２組の幾何モデルが交差するかを判定する物である． |
4 | | -irteusではノースカロライナ大学のLin氏らのグループにより開発されたPQPを |
5 | | -他言語インターフェースを介して利用できるようにしてある． |
6 | | -(他の干渉計算ソフトウェアパッケージについてはhttp://gamma.cs.unc.edu/research/collision/に詳しい．) |
7 | | -PQPは |
8 | | -(1)２つのモデルが交差するかを判定する衝突検出， |
9 | | -(2)２つのモデル間の最初距離を算出する距離計算， |
10 | | -(3)２つのモデルがある距離以下であるかを判定する近接検証， |
11 | | -等の３つ機能を提供する． |
| 3 | +\subsection{干渉計算の概要} |
| 4 | + |
| 5 | +干渉計算は２組の幾何モデルが交差するかを判定しその距離を求める処理である． |
| 6 | +主に以下の２つ機能を提供する． |
| 7 | +\begin{itemize} |
| 8 | + \item ２つのモデルが交差するかを判定する衝突検出 (collision-check関数) |
| 9 | + \item ２つのモデル間の最短距離を算出する距離計算 (collision-distance関数) |
| 10 | +\end{itemize} |
| 11 | + |
| 12 | +irteusでは，他言語インターフェースを介して外部ライブラリを呼び出すことで干渉計算を実行する． |
| 13 | +外部ライブラリとして，PQPとBulletの呼び出しが実装されており，デフォルトではPQPが利用される． |
| 14 | +以下のように，select-collision-algorithm関数により使用するライブラリを切り替えることができる． |
| 15 | +{\baselineskip=10pt |
| 16 | +\begin{verbatim} |
| 17 | +(select-collision-algorithm *collision-algorithm-pqp*) ;; use PQP |
| 18 | +(select-collision-algorithm *collision-algorithm-bullet*) ;; use Bullet |
| 19 | +\end{verbatim} |
| 20 | +} |
12 | 21 |
|
13 | | -PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに |
14 | | -書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになって |
15 | | -いる． |
| 22 | +個々の外部ライブラリの特徴については以降で詳しく説明する． |
| 23 | +他の干渉計算ソフトウェアパッケージについては http://gamma.cs.unc.edu/research/collision/に詳しい．（情報が古い可能性があるので注意．例えばBulletは載っていない．） |
| 24 | + |
| 25 | +\input{irtcollision-func} |
| 26 | + |
| 27 | +\subsubsection{物体形状モデル同士の干渉計算例} |
| 28 | +以下は，collision-checkやcollision-distanceを利用して，２つの立方体の衝突検出と距離計算を行い，最近点どうしを結ぶ線分を描画する例である． |
| 29 | +干渉が生じているときにcollision-distance関数で得られる最近点は，PQPとBulletで仕様が異なる．詳しくは以降のBulletに関する説明を参照． |
| 30 | +{\baselineskip=10pt |
| 31 | +\begin{verbatim} |
| 32 | +;; Make models |
| 33 | +(setq *b0* (make-cube 100 100 100)) |
| 34 | +(setq *b1* (make-cube 100 100 100)) |
| 35 | +
|
| 36 | +;; Case 1 : no collision |
| 37 | +(send *b0* :newcoords (make-coords :pos #f(100 100 -100) |
| 38 | + :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) |
| 39 | +(objects (list *b0* *b1*)) |
| 40 | +(print (collision-check *b0* *b1*)) ;; Check collision |
| 41 | +(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points |
| 42 | + (print (car ret)) ;; distance |
| 43 | + (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* |
| 44 | + (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* |
| 45 | + (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) |
| 46 | + (send *irtviewer* :viewer :viewsurface :flush)) |
| 47 | +
|
| 48 | +;; Case 2 : collision |
| 49 | +(send *b0* :newcoords (make-coords :pos #f(50 50 -50) |
| 50 | + :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) |
| 51 | +(objects (list *b0* *b1*)) |
| 52 | +(print (collision-check *b0* *b1*)) ;; Check collision |
| 53 | +(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points |
| 54 | + (print (car ret)) ;; distance |
| 55 | + ;; In case of collision, nearest points are insignificant values. |
| 56 | + (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* |
| 57 | + (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* |
| 58 | + (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) |
| 59 | + (send *irtviewer* :viewer :viewsurface :flush)) |
| 60 | +\end{verbatim} |
| 61 | +} |
| 62 | + |
| 63 | +\begin{figure}[htb] |
| 64 | + \begin{center} |
| 65 | + \includegraphics[width=0.50\columnwidth]{fig/collision.jpg} |
| 66 | + \caption{Collision detection} |
| 67 | + \end{center} |
| 68 | +\end{figure} |
| 69 | + |
| 70 | +\subsubsection{ロボット動作と干渉計算} |
| 71 | +ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつかむ動作を停止させるということが出来る． |
| 72 | + |
| 73 | +{\baselineskip=10pt |
| 74 | +\begin{verbatim} |
| 75 | +(load "irteus/demo/sample-arm-model.l") |
| 76 | +(setq *sarm* (instance sarmclass :init)) |
| 77 | +(send *sarm* :reset-pose) |
| 78 | +(setq a 42) |
| 79 | +(send *sarm* :move-fingers a) |
| 80 | +(setq *target* (make-cube 30 30 30)) |
| 81 | +(send *target* :translate #f(350 200 400)) |
| 82 | +(objects (list *sarm* *target*)) |
| 83 | +
|
| 84 | +(send *sarm* :inverse-kinematics *target* :move-target (send *sarm* :end-coords) :debug-view t) |
| 85 | +(while (> a 0) |
| 86 | + (if (collision-check-objects |
| 87 | + (list (send *sarm* :joint-fr :child-link) |
| 88 | + (send *sarm* :joint-fl :child-link)) |
| 89 | + (list *target*)) |
| 90 | + (return)) |
| 91 | + (decf a 0.1) |
| 92 | + (send *irtviewer* :draw-objects) |
| 93 | + (send *sarm* :move-fingers a)) |
| 94 | +(send *sarm* :end-coords :assoc *target*) |
| 95 | +
|
| 96 | +(dotimes (i 100) |
| 97 | + (send *sarm* :joint0 :joint-angle 1 :relative t) |
| 98 | + (send *irtviewer* :draw-objects)) |
| 99 | +(send *sarm* :end-coords :dissoc *target*) |
| 100 | +(dotimes (i 100) |
| 101 | + (send *sarm* :joint0 :joint-angle -1 :relative t) |
| 102 | + (send *irtviewer* :draw-objects)) |
| 103 | +\end{verbatim} |
| 104 | +} |
16 | 105 |
|
17 | | -\subsection{irteusからPQPの呼び出し} |
| 106 | +同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand, |
| 107 | +:close-handというメソッドで提供されている． |
| 108 | + |
| 109 | +\subsection{PQPによる干渉計算} |
| 110 | + |
| 111 | +PQPはノースカロライナ大学のLin氏らのグループにより開発された干渉計算ライブラリである． |
| 112 | +PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに |
| 113 | +書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになっている． |
18 | 114 |
|
19 | 115 | irteusでPQPを使うためのファイルは |
20 | 116 | CPQP.C, euspqp.c, pqp.l |
21 | 117 | からなる． |
22 | 118 | ２つの幾何モデルが衝突してしるか否かを判定するためには， |
23 | | - |
24 | 119 | {\baselineskip=10pt |
25 | 120 | \begin{verbatim} |
26 | 121 | (defun pqp-collision-check (model1 model2 |
@@ -90,74 +185,20 @@ \subsection{irteusからPQPの呼び出し} |
90 | 185 | る以降，(pqpbeginmodel m)でPQPの幾何モデルのインスタンスを作成し， |
91 | 186 | (pqpaddtri m v1 v2 v3 id)として面情報を登録している． |
92 | 187 |
|
93 | | -\subsubsection{物体形状モデル同士の干渉計算例} |
94 | | -pqp-collision-checkやpqp-collision-distanceを利用した例を示す． |
95 | | -{\baselineskip=10pt |
96 | | -\begin{verbatim} |
97 | | -;; Make models |
98 | | -(setq *b0* (make-cube 100 100 100)) |
99 | | -(setq *b1* (make-cube 100 100 100)) |
100 | | -
|
101 | | -;; Case 1 : no collision |
102 | | -(send *b0* :newcoords (make-coords :pos #f(100 100 -100) |
103 | | - :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) |
104 | | -(objects (list *b0* *b1*)) |
105 | | -(print (pqp-collision-check *b0* *b1*)) ;; Check collision |
106 | | -(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points |
107 | | - (print (car ret)) ;; distance |
108 | | - (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* |
109 | | - (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* |
110 | | - (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) |
111 | | - (send *irtviewer* :viewer :viewsurface :flush)) |
112 | | -
|
113 | | -;; Case 2 : collision |
114 | | -(send *b0* :newcoords (make-coords :pos #f(50 50 -50) |
115 | | - :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) |
116 | | -(objects (list *b0* *b1*)) |
117 | | -(print (pqp-collision-check *b0* *b1*)) ;; Check collision |
118 | | -(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points |
119 | | - (print (car ret)) ;; distance |
120 | | - ;; In case of collision, nearest points are insignificant values. |
121 | | - (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* |
122 | | - (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* |
123 | | - (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) |
124 | | - (send *irtviewer* :viewer :viewsurface :flush)) |
125 | | -\end{verbatim} |
126 | | -} |
127 | | - |
128 | | -\subsection{ロボット動作と干渉計算} |
129 | | - |
130 | | -ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に |
131 | | -手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつか |
132 | | -む動作を停止させるということが出来る． |
| 188 | +\input{pqp-func} |
133 | 189 |
|
134 | | -{\baselineskip=10pt |
135 | | -\begin{verbatim} |
136 | | -(objects (list *sarm* *target*)) |
| 190 | +\subsection{Bulletによる干渉計算} |
137 | 191 |
|
138 | | -(send *sarm* :solve-ik *target* :debug-view t) |
139 | | -(while (> a 0) |
140 | | - (if (pqp-collision-check-objects |
141 | | - (list (send *sarm* :joint-fr :child-link) |
142 | | - (send *sarm* :joint-fl :child-link)) |
143 | | - (list *target*)) |
144 | | - (return)) |
145 | | - (decf a 0.1) |
146 | | - (send *irtviewer* :draw-objects) |
147 | | - (send *sarm* :move-fingers a)) |
148 | | -(send *sarm* :end-coords :assoc *target*) |
149 | | -
|
150 | | -(dotimes (i 100) |
151 | | - (send *sarm* :joint0 :joint-angle 1 :relative t) |
152 | | - (send *irtviewer* :draw-objects)) |
153 | | -(send *sarm* :end-coords :dissoc *target*) |
154 | | -(dotimes (i 100) |
155 | | - (send *sarm* :joint0 :joint-angle -1 :relative t) |
156 | | - (send *irtviewer* :draw-objects)) |
157 | | -\end{verbatim} |
158 | | -} |
| 192 | +Bulletは物理演算エンジンであり，その一部として干渉計算機能が提供されている． |
| 193 | +irteusでBulletを使うためのファイルは |
| 194 | +CBULLET.cpp, eusbullet.c, bullet.l |
| 195 | +からなる． |
| 196 | +関数内部の呼び出し順序はPQPと同様である． |
159 | 197 |
|
160 | | -同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand, |
161 | | -:close-handというメソッドで提供されている． |
| 198 | +PQPとBulletの違いとして以下が挙げられる． |
| 199 | +\begin{itemize} |
| 200 | + \item 干渉が生じているときにcollision-distanceを呼ぶと，PQPでは，距離として0が返り，最近点として意味のない点が返される．一方Bulletでは，距離として干渉をなくすために動かすべき最短距離(penetration depthと呼ばれる)が返る．また，最近点としては，干渉をなくすための最短距離の両端の点が返される． |
| 201 | + \item PQPは非凸のメッシュモデルをそのまま扱うことができるが，Bulletでは非凸のモデルの凸包を内部で計算しそれを扱っている． |
| 202 | +\end{itemize} |
162 | 203 |
|
163 | | - \input{pqp-func} |
| 204 | +\input{bullet-func} |
0 commit comments