diff --git a/eus_qp/optmotiongen/README.md b/eus_qp/optmotiongen/README.md
index 2cd6d77d8..e32a8eda1 100644
--- a/eus_qp/optmotiongen/README.md
+++ b/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)
diff --git a/eus_qp/optmotiongen/doc/images/demo-pa10-surface-contact.gif b/eus_qp/optmotiongen/doc/images/demo-pa10-surface-contact.gif
new file mode 100644
index 000000000..eb3a8280c
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/demo-pa10-surface-contact.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/demo-pr2-holding-body-surface.gif b/eus_qp/optmotiongen/doc/images/demo-pr2-holding-body-surface.gif
new file mode 100644
index 000000000..8e4ce5f3a
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/demo-pr2-holding-body-surface.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/demo-rhp3-wholebody-contact-body-surface.gif b/eus_qp/optmotiongen/doc/images/demo-rhp3-wholebody-contact-body-surface.gif
new file mode 100644
index 000000000..d1cad79c8
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/demo-rhp3-wholebody-contact-body-surface.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-smooth-normal-link.png b/eus_qp/optmotiongen/doc/images/sample-smooth-normal-link.png
new file mode 100644
index 000000000..8637ac20f
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-smooth-normal-link.png differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-smooth-normal.png b/eus_qp/optmotiongen/doc/images/sample-smooth-normal.png
new file mode 100644
index 000000000..2a2e14ba7
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-smooth-normal.png differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-2.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-2.gif
new file mode 100644
index 000000000..ba7c99a8a
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-2.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-msc.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-msc.gif
new file mode 100644
index 000000000..0341abc8f
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface-msc.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface.gif
new file mode 100644
index 000000000..ed42b72c9
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-joint-surface.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-link.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-link.gif
new file mode 100644
index 000000000..c769abcfe
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-link.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-msc.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-msc.gif
new file mode 100644
index 000000000..771cb596b
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface-msc.gif differ
diff --git a/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface.gif b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface.gif
new file mode 100644
index 000000000..e5bc67bdb
Binary files /dev/null and b/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-surface.gif differ
diff --git a/eus_qp/optmotiongen/manual/Makefile b/eus_qp/optmotiongen/manual/Makefile
index 815020b5b..423e06831 100644
--- a/eus_qp/optmotiongen/manual/Makefile
+++ b/eus_qp/optmotiongen/manual/Makefile
@@ -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)
diff --git a/eus_qp/optmotiongen/manual/manual.pdf b/eus_qp/optmotiongen/manual/manual.pdf
index 96cbec316..b6cc80c14 100644
Binary files a/eus_qp/optmotiongen/manual/manual.pdf and b/eus_qp/optmotiongen/manual/manual.pdf differ
diff --git a/eus_qp/optmotiongen/manual/manual.tex b/eus_qp/optmotiongen/manual/manual.tex
index aff821e2c..359b7dbef 100644
--- a/eus_qp/optmotiongen/manual/manual.tex
+++ b/eus_qp/optmotiongen/manual/manual.tex
@@ -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}
diff --git a/eus_qp/optmotiongen/test/test-samplerobot.l b/eus_qp/optmotiongen/test/test-samplerobot.l
index df911c854..97ae1c3c9 100755
--- a/eus_qp/optmotiongen/test/test-samplerobot.l
+++ b/eus_qp/optmotiongen/test/test-samplerobot.l
@@ -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)