Merge pull request #555 from mmurooka/jskeus-bullet-c-defun

Add eusbullet for using bullet collision function (implement with c defun)

.travis-osx.sh

@@ -47,6 +47,7 @@ setup_make() {
     brew list mesalib-glw &>/dev/null || brew install mesalib-glw
     brew list wget &>/dev/null || brew install wget
     brew list poppler &>/dev/null || brew install poppler
+    brew list bullet &>/dev/null || brew install bullet
     travis_time_end
 
     # travis_time_start install.x11

.travis.sh

@@ -25,7 +25,7 @@ if [ "$(which sudo)" == "" ]; then apt-get update && apt-get install -y sudo; el
 travis_time_end
 
 travis_time_start setup.apt-get_install
-sudo apt-get install -qq -y git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng-dev xfonts-100dpi xfonts-75dpi  # msttcorefonts could not install on 14.04 travis
+sudo apt-get install -qq -y git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng-dev xfonts-100dpi xfonts-75dpi pkg-config libbullet-dev  # msttcorefonts could not install on 14.04 travis
 # sudo apt-get install -qq -y texlive-latex-base ptex-bin latex2html nkf poppler-utils || echo "ok" # 16.04 does ont have ptex bin
 travis_time_end
 
@@ -51,6 +51,9 @@ for test_l in irteus/test/*.l; do
 
     # osrf/ubuntu_arm64:trusty takes >50 min, skip irteus-demo.l
     [[ "$DOCKER_IMAGE" == *"arm64:trusty"* && $test_l =~ irteus-demo.l ]] && continue;
+    # skip collision test because bullet of 2.83 or later version is not released in trusty and jessie.
+    # https://github.com/euslisp/jskeus/blob/6cb08aa6c66fa8759591de25b7da68baf76d5f09/irteus/Makefile#L37
+    [[ ( "$DOCKER_IMAGE" == *"trusty"* || "$DOCKER_IMAGE" == *"jessie"* ) && $test_l =~ test-collision.l ]] && continue;
 
     travis_time_start jskeus.source.${test_l##*/}.test
 

.travis.yml

@@ -75,7 +75,7 @@ script:
   # Test installing head version jskeus via Homebrew formula
   - if [ "$TRAVIS_OS_NAME" = "osx" ]; then $CI_SOURCE_PATH/.travis-osx.sh; fi
   # Test doc generation
-  - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi; fi
+  - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi pkg-config libbullet-dev; fi
   - if [ "$BUILD_DOC" == "true" ]; then make; fi
   - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq texlive-binaries texlive-lang-cjk poppler-utils nkf latex2html; fi
   - if [ "$BUILD_DOC" == "true" ]; then (source bashrc.eus; cd doc/; make pdf); fi

README.md

@@ -127,12 +127,12 @@ PDF files are also available from [here](https://github.com/euslisp/jskeus/raw/m
 
 For Ubuntu users:
 ```
-$ sudo apt-get install git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi gsfonts-x11 texlive-fonts-extra xfonts-100dpi-transcoded xfonts-75dpi-transcoded msttcorefonts
+$ sudo apt-get install git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi gsfonts-x11 texlive-fonts-extra xfonts-100dpi-transcoded xfonts-75dpi-transcoded msttcorefonts pkg-config libbullet-dev
 ```
 
 For Mac OSX users using Homebrew:
 ```
-$ brew install jpeg libpng mesalib-glw wget
+$ brew install jpeg libpng mesalib-glw wget bullet
 ```
 
 '''NOTE:'''

doc/Makefile

@@ -22,7 +22,7 @@ copy_eus_tex:
 	(cd ${TMPDIR};for x in fig/*.jpg; do extractbb $$x; done) # workaround for https://github.com/backtracking/bibtex2html/issues/9
 	cp *.tex ${TMPDIR}
 	platex --version | grep utf8 || nkf --in-place -e ${TMPDIR}/*.tex
-	for x in irtrobot irtmodel irtsensor irtscene irtdyna irtgeo irtgl irtutil irtviewer irtx irtmath irtbvh irtcollada irtgraph irtimage irtpointcloud png pqp; do\
+	for x in irtrobot irtmodel irtsensor irtscene irtdyna irtgeo irtgl irtutil irtviewer irtx irtmath irtbvh irtcollada irtgraph irtimage irtpointcloud png irtcollision pqp bullet; do\
 		irteusgl ../eus/lib/llib/documentation.l "(make-document \"../irteus/$$x.l\" \"${TMPDIR}/$$x-func.tex\")" "(exit)"; \
         done
 

doc/pqp.tex → doc/irtcollision.tex

@@ -1,26 +1,121 @@
 \section{干渉計算}
 
-干渉計算には２組の幾何モデルが交差するかを判定する物である．
-irteusではノースカロライナ大学のLin氏らのグループにより開発されたPQPを
-他言語インターフェースを介して利用できるようにしてある．
-(他の干渉計算ソフトウェアパッケージについてはhttp://gamma.cs.unc.edu/research/collision/に詳しい．)
-PQPは
-(1)２つのモデルが交差するかを判定する衝突検出，
-(2)２つのモデル間の最初距離を算出する距離計算，
-(3)２つのモデルがある距離以下であるかを判定する近接検証，
-等の３つ機能を提供する．
+\subsection{干渉計算の概要}
+
+干渉計算は２組の幾何モデルが交差するかを判定しその距離を求める処理である．
+主に以下の２つ機能を提供する．
+\begin{itemize}
+  \item ２つのモデルが交差するかを判定する衝突検出 (collision-check関数)
+  \item ２つのモデル間の最短距離を算出する距離計算 (collision-distance関数)
+\end{itemize}
+
+irteusでは，他言語インターフェースを介して外部ライブラリを呼び出すことで干渉計算を実行する．
+外部ライブラリとして，PQPとBulletの呼び出しが実装されており，デフォルトではPQPが利用される．
+以下のように，select-collision-algorithm関数により使用するライブラリを切り替えることができる．
+{\baselineskip=10pt
+\begin{verbatim}
+(select-collision-algorithm *collision-algorithm-pqp*) ;; use PQP
+(select-collision-algorithm *collision-algorithm-bullet*) ;; use Bullet
+\end{verbatim}
+}
 
-PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに
-書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになって
-いる．
+個々の外部ライブラリの特徴については以降で詳しく説明する．
+他の干渉計算ソフトウェアパッケージについては http://gamma.cs.unc.edu/research/collision/に詳しい．（情報が古い可能性があるので注意．例えばBulletは載っていない．）
+
+\input{irtcollision-func}
+
+\subsubsection{物体形状モデル同士の干渉計算例}
+以下は，collision-checkやcollision-distanceを利用して，２つの立方体の衝突検出と距離計算を行い，最近点どうしを結ぶ線分を描画する例である．
+干渉が生じているときにcollision-distance関数で得られる最近点は，PQPとBulletで仕様が異なる．詳しくは以降のBulletに関する説明を参照．
+{\baselineskip=10pt
+\begin{verbatim}
+;; Make models
+(setq *b0* (make-cube 100 100 100))
+(setq *b1* (make-cube 100 100 100))
+
+;; Case 1 : no collision
+(send *b0* :newcoords (make-coords :pos #f(100 100 -100)
+                                   :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30))))
+(objects (list *b0* *b1*))
+(print (collision-check *b0* *b1*)) ;; Check collision
+(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points
+  (print (car ret)) ;; distance
+  (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0*
+  (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1*
+  (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret))
+  (send *irtviewer* :viewer :viewsurface :flush))
+
+;; Case 2 : collision
+(send *b0* :newcoords (make-coords :pos #f(50 50 -50)
+                                   :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30))))
+(objects (list *b0* *b1*))
+(print (collision-check *b0* *b1*)) ;; Check collision
+(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points
+  (print (car ret)) ;; distance
+  ;; In case of collision, nearest points are insignificant values.
+  (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0*
+  (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1*
+  (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret))
+  (send *irtviewer* :viewer :viewsurface :flush))
+\end{verbatim}
+}
+
+\begin{figure}[htb]
+  \begin{center}
+    \includegraphics[width=0.50\columnwidth]{fig/collision.jpg}
+    \caption{Collision detection}
+  \end{center}
+\end{figure}
+
+\subsubsection{ロボット動作と干渉計算}
+ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつかむ動作を停止させるということが出来る．
+
+{\baselineskip=10pt
+\begin{verbatim}
+(load "irteus/demo/sample-arm-model.l")
+(setq *sarm* (instance sarmclass :init))
+(send *sarm* :reset-pose)
+(setq a 42)
+(send *sarm* :move-fingers a)
+(setq *target* (make-cube 30 30 30))
+(send *target* :translate #f(350 200 400))
+(objects (list *sarm* *target*))
+
+(send *sarm* :inverse-kinematics *target* :move-target (send *sarm* :end-coords) :debug-view t)
+(while (> a 0)
+  (if (collision-check-objects
+       (list (send *sarm* :joint-fr :child-link)
+             (send *sarm* :joint-fl :child-link))
+       (list *target*))
+      (return))
+  (decf a 0.1)
+  (send *irtviewer* :draw-objects)
+  (send *sarm* :move-fingers a))
+(send *sarm* :end-coords :assoc *target*)
+
+(dotimes (i 100)
+  (send *sarm* :joint0 :joint-angle 1 :relative t)
+  (send *irtviewer* :draw-objects))
+(send *sarm* :end-coords :dissoc *target*)
+(dotimes (i 100)
+  (send *sarm* :joint0 :joint-angle -1 :relative t)
+  (send *irtviewer* :draw-objects))
+\end{verbatim}
+}
 
-\subsection{irteusからPQPの呼び出し}
+同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand,
+:close-handというメソッドで提供されている．
+
+\subsection{PQPによる干渉計算}
+
+PQPはノースカロライナ大学のLin氏らのグループにより開発された干渉計算ライブラリである．
+PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに
+書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになっている．
 
 irteusでPQPを使うためのファイルは
 CPQP.C, euspqp.c, pqp.l
 からなる．
 ２つの幾何モデルが衝突してしるか否かを判定するためには，
-
 {\baselineskip=10pt
 \begin{verbatim}
 (defun pqp-collision-check (model1 model2
@@ -90,74 +185,20 @@ \subsection{irteusからPQPの呼び出し}
 る以降，(pqpbeginmodel m)でPQPの幾何モデルのインスタンスを作成し，
 (pqpaddtri m v1 v2 v3 id)として面情報を登録している．
 
-\subsubsection{物体形状モデル同士の干渉計算例}
-pqp-collision-checkやpqp-collision-distanceを利用した例を示す．
-{\baselineskip=10pt
-\begin{verbatim}
-;; Make models
-(setq *b0* (make-cube 100 100 100))
-(setq *b1* (make-cube 100 100 100))
-
-;; Case 1 : no collision
-(send *b0* :newcoords (make-coords :pos #f(100 100 -100)
-                                   :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30))))
-(objects (list *b0* *b1*))
-(print (pqp-collision-check *b0* *b1*)) ;; Check collision
-(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points
-  (print (car ret)) ;; distance
-  (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0*
-  (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1*
-  (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret))
-  (send *irtviewer* :viewer :viewsurface :flush))
-
-;; Case 2 : collision
-(send *b0* :newcoords (make-coords :pos #f(50 50 -50)
-                                   :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30))))
-(objects (list *b0* *b1*))
-(print (pqp-collision-check *b0* *b1*)) ;; Check collision
-(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points
-  (print (car ret)) ;; distance
-  ;; In case of collision, nearest points are insignificant values.
-  (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0*
-  (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1*
-  (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret))
-  (send *irtviewer* :viewer :viewsurface :flush))
-\end{verbatim}
-}
-
-\subsection{ロボット動作と干渉計算}
-
-ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に
-手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつか
-む動作を停止させるということが出来る．
+\input{pqp-func}
 
-{\baselineskip=10pt
-\begin{verbatim}
-(objects (list *sarm* *target*))
+\subsection{Bulletによる干渉計算}
 
-(send *sarm* :solve-ik *target* :debug-view t)
-(while (> a 0)
-  (if (pqp-collision-check-objects
-       (list (send *sarm* :joint-fr :child-link)
-             (send *sarm* :joint-fl :child-link))
-       (list *target*))
-      (return))
-  (decf a 0.1)
-  (send *irtviewer* :draw-objects)
-  (send *sarm* :move-fingers a))
-(send *sarm* :end-coords :assoc *target*)
-
-(dotimes (i 100)
-  (send *sarm* :joint0 :joint-angle 1 :relative t)
-  (send *irtviewer* :draw-objects))
-(send *sarm* :end-coords :dissoc *target*)
-(dotimes (i 100)
-  (send *sarm* :joint0 :joint-angle -1 :relative t)
-  (send *irtviewer* :draw-objects))
-\end{verbatim}
-}
+Bulletは物理演算エンジンであり，その一部として干渉計算機能が提供されている．
+irteusでBulletを使うためのファイルは
+CBULLET.cpp, eusbullet.c, bullet.l
+からなる．
+関数内部の呼び出し順序はPQPと同様である．
 
-同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand,
-:close-handというメソッドで提供されている．
+PQPとBulletの違いとして以下が挙げられる．
+\begin{itemize}
+  \item 干渉が生じているときにcollision-distanceを呼ぶと，PQPでは，距離として0が返り，最近点として意味のない点が返される．一方Bulletでは，距離として干渉をなくすために動かすべき最短距離(penetration depthと呼ばれる)が返る．また，最近点としては，干渉をなくすための最短距離の両端の点が返される．
+  \item PQPは非凸のメッシュモデルをそのまま扱うことができるが，Bulletでは非凸のモデルの凸包を内部で計算しそれを扱っている．
+\end{itemize}
 
- \input{pqp-func}
+\input{bullet-func}

doc/jmanual.tex

@@ -231,8 +231,10 @@ \part{irteus 拡張}
 \input{irtviewer}
 %% irtviewer.l
 
-\input{pqp}
+\input{irtcollision}
+%% irtcollision.l
 %% pqp.l
+%% bullet.l
 
 \input{irtbvh}