optmotiongen: update test, manual pdf, and README for optmotiongen with the body surface contacts.

Author: mmurooka

eus_qp/optmotiongen/README.md

@@ -278,6 +278,65 @@ Gif images are generated by [generate-gif.l](doc/generate-gif.l). Gif animation
 ```
 ![sample-robot-sqp-bspline-dynamic-config-task](doc/images/sample-robot-sqp-bspline-dynamic-config-task.gif)
 
+### sample-smooth-kinematics
+
+[sample-smooth-kinematics.l](./euslisp/sample/sample-smooth-kinematics.l)
+
+###### sample-smooth-normal
+```
+(sample-smooth-normal)
+```
+![sample-smooth-normal](doc/images/sample-smooth-normal.png)
+
+###### sample-smooth-normal-link
+```
+(sample-smooth-normal :body-type :link)
+```
+![sample-smooth-normal-link](doc/images/sample-smooth-normal-link.png)
+
+### sample-sqp-optimization-surface
+
+[sample-sqp-optimization-surface.l](./euslisp/sample/sample-sqp-optimization-surface.l)
+
+###### sample-sqp-optimization-surface
+```
+(sample-sqp-optimization-surface)
+```
+![sample-sqp-optimization-surface](doc/images/sample-sqp-optimization-surface.gif)
+
+###### sample-sqp-optimization-surface-link
+```
+(sample-sqp-optimization-surface :body-type :link)
+```
+![sample-sqp-optimization-surface-link](doc/images/sample-sqp-optimization-surface-link.gif)
+
+###### sample-sqp-optimization-surface-msc
+```
+(sample-sqp-optimization-surface :use-msc? t)
+```
+![sample-sqp-optimization-surface-msc](doc/images/sample-sqp-optimization-surface-msc.gif)
+
+### sample-sqp-optimization-joint-surface.l
+
+[sample-sqp-optimization-joint-surface.l](./euslisp/sample/sample-sqp-optimization-joint-surface.l)
+
+###### sample-sqp-optimization-joint-surface
+```
+(sample-sqp-optimization-joint-surface :loop-num 150)
+```
+![sample-sqp-optimization-joint-surface](doc/images/sample-sqp-optimization-joint-surface.gif)
+
+```
+(sample-sqp-optimization-joint-surface :target-coords (make-coords :pos (float-vector 1000 -800 600)))
+```
+![sample-sqp-optimization-joint-surface-2](doc/images/sample-sqp-optimization-joint-surface-2.gif)
+
+###### sample-sqp-optimization-joint-surface-msc
+```
+(sample-sqp-optimization-joint-surface :use-msc? t)
+```
+![sample-sqp-optimization-joint-surface-msc](doc/images/sample-sqp-optimization-joint-surface-msc.gif)
+
 
 ## Sample (HRP2)
 
@@ -465,3 +524,33 @@ Gif images are generated by [generate-gif.l](doc/generate-gif.l). Gif animation
 (demo-2d-body-trajectory :num-of-solution-candidates 1)
 ```
 ![demo-2d-body-trajectory-without-msc](doc/images/demo-2d-body-trajectory-without-msc.gif)
+
+### demo-pa10-surface-contact
+
+[demo-pa10-surface-contact.l](./euslisp/demo/demo-pa10-surface-contact.l)
+
+###### demo-pa10-surface-contact
+```
+(demo-pa10-surface-contact)
+```
+![demo-pa10-surface-contact](doc/images/demo-pa10-surface-contact.gif)
+
+### demo-rhp3-wholebody-contact
+
+[demo-rhp3-wholebody-contact.l](./euslisp/demo/demo-rhp3-wholebody-contact.l)
+
+###### demo-rhp3-wholebody-contact-body-surface
+```
+(demo-rhp3-wholebody-contact-body-surface)
+```
+![demo-rhp3-wholebody-contact-body-surface](doc/images/demo-rhp3-wholebody-contact-body-surface.gif)
+
+### demo-pr2-holding
+
+[demo-pr2-holding.l](./euslisp/demo/demo-pr2-holding.l)
+
+###### demo-pr2-holding-body-surface
+```
+(demo-pr2-holding-body-surface)
+```
+![demo-pr2-holding-body-surface](doc/images/demo-pr2-holding-body-surface.gif)

eus_qp/optmotiongen/doc/images/demo-pa10-surface-contact.gif

@@ -1,6 +1,6 @@
 TMPDIR := $(shell mktemp -d)
 FIGDIR := ${TMPDIR}/fig
-EUSFILES = instant-configuration-task trajectory-configuration-task sqp-optimization robot-object-environment instant-manipulation-configuration-task bspline-fundamental bspline-configuration-task extended-joint-link contact-kinematics robot-environment torque-gradient inverse-kinematics-wrapper sqp-msc-optimization bspline-dynamic-configuration-task discrete-kinematics-configuration-task
+EUSFILES = instant-configuration-task trajectory-configuration-task sqp-optimization robot-object-environment instant-manipulation-configuration-task bspline-fundamental bspline-configuration-task extended-joint-link contact-kinematics robot-environment torque-gradient inverse-kinematics-wrapper sqp-msc-optimization bspline-dynamic-configuration-task discrete-kinematics-configuration-task smooth-kinematics surface-configuration-task joint-surface-configuration-task
 EUSFILESDIR = "../euslisp"
 
 # IMAGE_FILES := $(shell find ${FIGDIR} -name "*.(jpg|png)" | sed s/jpg/bb/g)

eus_qp/optmotiongen/doc/images/demo-pr2-holding-body-surface.gif

@@ -227,6 +227,14 @@ \subsubsection{離散的な幾何目標に対する逆運動学計算の理論}
 \input{theory-discrete-ik}
 \subsubsection{離散的な幾何目標に対する逆運動学計算の実装}
 \input{discrete-kinematics-configuration-task}
+%%%%%
+\subsection{ボディ表面のコンフィギュレーションとタスク関数} \label{sec:body-surface}
+\subsubsection{ボディ表面の連続関数近似}
+\input{smooth-kinematics}
+\subsubsection{ボディ表面}
+\input{surface-configuration-task}
+\subsubsection{関節とボディ表面}
+\input{joint-surface-configuration-task}
 
 %%%%%%%%%%%%%%%%%%%%%%
 \section{補足} \label{chap:appendix}

eus_qp/optmotiongen/doc/images/demo-rhp3-wholebody-contact-body-surface.gif

@@ -2,6 +2,7 @@
 
 (require :unittest "lib/llib/unittest.l")
 (load "../euslisp/sample/sample-samplerobot.l")
+(load "../euslisp/sample/sample-sqp-optimization-joint-surface.l")
 
 
 (init-unit-test)
@@ -38,6 +39,11 @@
   (assert (< (norm (send *bspline-dynamic-config-task* :task-value :update? nil)) 100.0))
   )
 
+(deftest test-sample-robot-sqp-joint-surface-config-task
+  (null-output (sample-sqp-optimization-joint-surface))
+  (assert (< (norm (send *joint-surface-config-task* :task-value :update? nil)) 1.0))
+  )
+
 
 (eval-when
  (load eval)