Merge pull request #550 from Naoki-Hiraoka/add-documentation-of-gg

add documentation of :calc-walk-pattern-from-footstep-list

Author: k-okada

doc/fig/walk-motion.jpg

@@ -2185,4 +2185,230 @@ \subsubsection{外力を考慮したfullbody-inverse-kinematicsを解く例}
  \subsection{環境モデル}
   \input{irtscene-func}
  \subsection{動力学計算・歩行動作生成}
+\subsubsection{歩行動作生成}
+歩行動作は予見制御を用いて生成される。文献\footnote{
+Humanoid Robot (in Japanese), Shuji Kajita, Ohmsha, 2005, ISBN 4-274-20058-2
+}\footnote{
+Biped Walking Pattern Generation by using Preview Control of Zero-Moment Point, Shuji Kajita and Fumio Kanehiro and Kenji Kaneko and Kiyoshi Fujiwara and Kensuke Harada and Kazuhito Yokoi and Hirohisa Hirukawa, ICRA 2003, p.1620-1626, 2006
+}\footnote{
+最適予見制御と一般化予測制御, 江上 正 and土谷 武士, 計測と制御, 39 巻, 5 号, p.337-342, 2000
+}に述べられている式や文章を引用しつつ、説明する。
+
+歩行動作は以下の手順で生成される。
+\begin{enumerate}
+\item 脚の逆運動学が解ける範囲内の歩幅で、ロボットが足をつく位置と時間を計画する。
+\item 地面についていない足は、次につく位置までサイクロイド補間する。
+\item ZMPが地面についている足の位置とできる限り一致するような重心軌道を生成する。
+\item 計画された足と重心の軌道を満たすような関節角度軌道を逆運動学によって逐次求める。
+\end{enumerate}
+ここでは、3の手順について詳しく説明する。
+
+X方向の運動について、始めに次のような制御系を考える。Y方向の運動についても同様に議論することが可能である。
+\begin{eqnarray}
+  && \left\{
+  \begin{array}{l}
+    x_{k+1} = A x_k + b u_k \\
+    p_k = c x_k
+  \end{array}
+  \right. \eqlabel{preview_control}\\
+  && x_k \equiv \left[
+    \begin{array}{c}
+      x(k\Delta t)\\
+      \dot x(k\Delta t)\\
+      \ddot x(k\Delta t)
+    \end{array}
+    \right] ~~~~ u_k \equiv u(k\Delta t) ~~~~ p_k \equiv p(k\Delta t)\nonumber\\
+  && A \equiv \left[
+    \begin{array}{c c c}
+      1 & \Delta t & \Delta t^2 / 2\\
+      0 & 1 & \Delta t\\
+      0 & 0 & 1
+    \end{array}
+    \right] ~~~~ b \equiv \left[
+    \begin{array}{c}
+      \Delta t^3 / 6\\
+      \Delta t^2 / 2\\
+      \Delta t
+    \end{array}
+    \right] ~~~~ c \equiv \left[
+    \begin{array}{c c c}
+      1 & 0 & -z_c/g
+    \end{array}
+    \right]\nonumber
+\end{eqnarray}
+$x$はロボットの重心位置、$u$は重心の加速度の時間微分(jerk)、$p$はZMPである。
+
+次に、このシステムを次のような拡大系に置き換える。
+\begin{eqnarray}
+  && \left\{
+  \begin{array}{l}
+    x^\ast_{k+1} = \tilde{A} x^\ast_k + \tilde{b} \Delta u_k \\
+    p_k = \tilde{c} x^\ast_k
+  \end{array}
+  \right. \eqlabel{extended_preview_control}\\
+  && \Delta u_k \equiv u_k - u_{k-1} ~~~~ \Delta x_k \equiv x_k - x_{k-1} ~~~~ x^\ast_k \equiv \left[
+    \begin{array}{c}
+      p_k\\
+      \Delta x_k
+    \end{array}
+    \right] \nonumber\\
+  && \tilde{A} \equiv \left[
+    \begin{array}{c c}
+      1 & cA\\
+      0 & A
+    \end{array}
+    \right] ~~~~ \tilde{b} \equiv \left[
+    \begin{array}{c}
+      cb\\
+      b
+    \end{array}
+    \right] ~~~~ \tilde{c} \equiv \left[
+    \begin{array}{c c c c}
+      1 & 0 & 0 & 0
+    \end{array}
+    \right]\nonumber
+\end{eqnarray}
+この\eqref{extended_preview_control}のシステムに対して、次のコスト関数を最小化するような制御入力を求める。
+\begin{eqnarray}
+  J_k = \sum_{j=k}^{\infty} \{Q(p_{j}^{ref} - p_{j})^2 + R\Delta u_{j}^2\} \eqlabel{extended_preview_control_costfunc}
+\end{eqnarray}
+これは、以下のようにして求められる。
+まず、十分大きい自然数$M$を用いて次のコスト関数を考え、次に$M \rightarrow \infty$とすることで\eqref{extended_preview_control_costfunc}を最小化する制御入力を得る。
+\begin{eqnarray}
+  J_k^M = \sum_{j=k}^{M-1} \{Q(p_{j}^{ref} - p_{j})^2 + R\Delta u_{j}^2\} \eqlabel{extended_preview_control_costfuncM}
+\end{eqnarray}
+$J_k^M$の最小値を$J_k^{M\ast}$とすると、\eqref{extended_preview_control_costfuncM}から次の関係が成り立つ。
+\begin{eqnarray}
+  J_k^{M\ast} = \min_{\Delta u_k} \{Q(p_k^{ref} - p_k)^2 + R\Delta u_k^2 + J_{k+1}^{M\ast}\}  \eqlabel{extended_preview_control_costfunc2}
+\end{eqnarray}
+ここで、$J_k^{M\ast}$を次のようにおく。
+\begin{eqnarray}
+  J_k^{M\ast} = x_k^{\ast T} P_k^M x_k^\ast + \theta_k^{MT} x_k^\ast + \phi_k^M \eqlabel{extended_preview_control_J}
+\end{eqnarray}
+これを用いて、\eqref{extended_preview_control_costfunc2}の右辺を$\Delta u_k$で偏微分した値が0であることから、次の式を得る。
+\begin{eqnarray}
+  0 = && \frac{\partial}{\partial \Delta u_k} \{ Q(p_k^{ref} - p_k)^2 + R\Delta u_k^2 + (\tilde{A} x^\ast_k + \tilde{b} \Delta u_k)^T P_{k+1}^M (\tilde{A} x^\ast_k + \tilde{b} \Delta u_k) + \theta_{k+1}^{MT} (\tilde{A} x^\ast_k + \tilde{b} \Delta u_k) + \phi_{k+1}^M \} \nonumber\\
+  0 =&& \Delta u_k^T R + \Delta u_k^T \tilde{b}^T P_{k+1}^M \tilde{b} + x_k^{\ast T} \tilde{A}^T P_{k+1}^M \tilde{b} + \frac{1}{2} \theta_{k+1}^{MT} \tilde{b} \nonumber \\
+  \Delta u_k =&& -(\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T P_{k+1}^M \tilde{A} x_k^\ast - \frac{1}{2} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T \theta_{k+1}^M\eqlabel{extended_preview_control_u}
+\end{eqnarray}
+これを\eqref{extended_preview_control_costfunc2}に代入することで、以下を得る。
+\begin{eqnarray}
+  && x_k^{\ast T} P_k^M x_k^\ast + \theta_k^{M T} x_k^\ast + \phi_k^M \nonumber \\
+  =&& x_k^{\ast T} (\tilde{c}^T Q \tilde{c} + \tilde{A}^T P_{k+1}^M \tilde{A} - \tilde{A}^T P_{k+1}^M \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T P_{k+1}^M \tilde{A}) x_k^\ast \nonumber\\
+  && + \{-2 \tilde{c}^T Q p_k^{ref} + \tilde{A}^T \theta_{k+1}^M - \tilde{A}^T P_{k+1}^M \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T \theta_{k+1}^M \}^T x_k^\ast \nonumber\\
+  && + Q p_k^{ref 2} - \frac{1}{4} \theta_{k+1}^{MT} \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T \theta_{k+1}^M + \phi_{k+1}^M
+\end{eqnarray}
+これが任意の$x_k^{\ast T}$に対し成り立つので、
+\begin{eqnarray}
+  P_k^M &=& \tilde{c}^T Q \tilde{c} + \tilde{A}^T P_{k+1}^M \tilde{A} - \tilde{A}^T P_{k+1}^M \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T P_{k+1}^M \tilde{A} \eqlabel{extended_preview_control_P}\\
+  \theta_k^M &=& -2 \tilde{c}^T Q p_k^{ref} + \{\tilde{A}^T - \tilde{A}^T P_{k+1}^M \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T\} \theta_{k+1}^M \eqlabel{extended_preview_control_theta}\\
+  \phi_k^M &=& Q p_k^{ref 2} - \frac{1}{4} \theta_{k+1}^{MT} \tilde{b} (\tilde{b}^T P_{k+1}^M \tilde{b} + R )^{-1} \tilde{b}^T \theta_{k+1}^M + \phi_{k+1}^M
+\end{eqnarray}
+境界条件を考えると、\eqref{extended_preview_control_costfuncM}は$k = M$において$J_{k+M}^{M\ast} = 0$であるから、$k = M$において\eqref{extended_preview_control_J}が任意の$x_{M}^\ast$で恒等的に成り立つ条件を考えて、$P_{M}^M = 0$、$\theta_{M}^M = 0$、$\phi_{M}^M = 0$である。したがって、十分小さい$j$において$P_{k+j}^M$は\eqref{extended_preview_control_P}の定常解$P$となり、次の式を満たす。
+\begin{eqnarray}
+  P &=& \tilde{c}^T Q \tilde{c} + \tilde{A}^T P \tilde{A} - \tilde{A}^T P \tilde{b} (\tilde{b}^T P \tilde{b} + R )^{-1} \tilde{b}^T P \tilde{A} \eqlabel{extended_preview_control_P2}
+\end{eqnarray}
+以後、$M \rightarrow \infty$として考える。自然数$N$を考えて、$j > N$において$p_{k+j}^{ref} = p_{k+N}^{ref}$と仮定すると、\eqref{extended_preview_control_theta}は次のように表せる。(\eqref{extended_preview_control_P2}は十分小さい$j$における$P_{k+j}$でのみ成り立たつが、十分大きな$j$では$(\tilde{A} - \tilde{b} K)^{T j} \rightarrow 0$と仮定し無視した。)($K \equiv (\tilde{b}^T P \tilde{b} + R )^{-1} \tilde{b}^T P \tilde{A}$とおいた。)
+\begin{eqnarray}
+  \theta_k &=& -2 \tilde{c}^T Q p_k^{ref} + (\tilde{A} - \tilde{b} K)^T \theta_{k+1} \nonumber\\
+  &=& -2 \{ \tilde{c}^T Q p_k^{ref} + (\tilde{A} - \tilde{b} K)^T \tilde{c}^T Q p_{k+1}^{ref} + \cdots + (\tilde{A} - \tilde{b} K)^{T N-2} \tilde{c}^T Q p_{k+N-1}^{ref} \nonumber\\
+  & & + (\tilde{A} - \tilde{b} K)^{T N-1} \tilde{c}^T Q p_{k+N}^{ref} + (\tilde{A} - \tilde{b} K)^{T N} \tilde{c}^T Q p_{k+N+1}^{ref} + (\tilde{A} - \tilde{b} K)^{T (N+1)} \tilde{c}^T Q p_{k+N+2}^{ref} + \cdots \} \nonumber \\
+  &=& -2 \{ \tilde{c}^T Q p_k^{ref} + (\tilde{A} - \tilde{b} K)^T \tilde{c}^T Q p_{k+1}^{ref} + \cdots + (\tilde{A} - \tilde{b} K)^{T N-2} \tilde{c}^T Q p_{k+N-1}^{ref} \nonumber\\
+  & & + (\tilde{A} - \tilde{b} K)^{T N-1} \tilde{c}^T Q p_{k+N}^{ref} + (\tilde{A} - \tilde{b} K)^{T N} \tilde{c}^T Q p_{k+N}^{ref} + (\tilde{A} - \tilde{b} K)^{T (N+1)} \tilde{c}^T Q p_{k+N}^{ref} + \cdots \} \nonumber \\
+  &=& -2 \sum_{j=1}^{N-1} \{ (\tilde{A} - \tilde{b} K)^{T j-1} \tilde{c}^T Q p_{k+j}^{ref} \} -2 \sum_{j=N}^{\infty} \{ (\tilde{A} - \tilde{b} K)^{T j-1} \tilde{c}^T Q p_{k+N}^{ref} \}\eqlabel{extended_preview_control_theta2}
+\end{eqnarray}
+ここで\eqref{extended_preview_control_P2}を$K$を用いて表現すると、
+\begin{eqnarray}
+  P &=& \tilde{c}^T Q \tilde{c} + (\tilde{A} - \tilde{b} K)^T P \tilde{A} \eqlabel{extended_preview_control_P3}
+\end{eqnarray}
+が得られる。この両辺に$P \tilde{A}$を加え整理すると、
+\begin{eqnarray}
+  (I - (\tilde{A} - \tilde{b} K)^{T}) P \tilde{A} &=& P (\tilde{A} - I) + \tilde{c}^T Q \tilde{c} \eqlabel{extended_preview_control_P4}
+\end{eqnarray}
+となる。ここで$\tilde{A} = \left[\begin{array}{c c} 1 & cA\\ 0 & A \end{array} \right]$、$\tilde{c} = \left[\begin{array}{c c c c} 1 & 0 & 0 & 0 \end{array} \right]$を利用すると、\eqref{extended_preview_control_P4}の第1列目の等式から、
+\begin{eqnarray}
+  (I - (\tilde{A} - \tilde{b} K)^{T}) P \tilde{c}^T &=& \tilde{c}^T Q \eqlabel{extended_preview_control_P5}
+\end{eqnarray}
+が成り立つ。この\eqref{extended_preview_control_P5}を\eqref{extended_preview_control_theta2}に代入すると、
+\begin{eqnarray}
+  \theta_k &=& -2 \sum_{j=1}^{N-1} \{ (\tilde{A} - \tilde{b} K)^{T j-1} \tilde{c}^T Q p_{k+j}^{ref} \} -2 (\tilde{A} - \tilde{b} K)^{T N-1} P \tilde{c}^T p_{k+N}^{ref} \eqlabel{extended_preview_control_theta3}
+\end{eqnarray}
+を得る。この\eqref{extended_preview_control_theta3}を\eqref{extended_preview_control_u}に代入することで、\eqref{extended_preview_control_costfunc}を最小化する最適制御入力$\Delta u_k$が得られる。
+\begin{eqnarray}
+  \Delta u_k &=& -K x_k^\ast + \sum_{j=1}^N \tilde{f}_j p_{k+j}^{ref}\eqlabel{extended_preview_control_u2}\\
+  \tilde{f}_j &=& \begin{cases}
+    (\tilde{b}^T P \tilde{b} + R )^{-1} \tilde{b}^T (\tilde{A} - \tilde{b} K)^{T j-1} \tilde{c}^T Q & (j<N) \\
+    (\tilde{b}^T P \tilde{b} + R )^{-1} \tilde{b}^T (\tilde{A} - \tilde{b} K)^{T N-1} P \tilde{c}^T & (j=N)
+  \end{cases}\nonumber
+\end{eqnarray}
+初期状態$x_1^\ast$と目標ZMP軌道$p_{1}^{ref},~p_{2}^{ref},~\cdots$が与えられれば、\eqref{extended_preview_control_u2}と\eqref{extended_preview_control}のシステムによって$\Delta u_1,~x_2^\ast,~\Delta u_2,~x_3^\ast,~\cdots$が順次求まる。これによって、ZMPが地面についている足の位置とできる限り一致するような重心軌道を生成することができる。
+
+\subsubsection{歩行動作生成の例}
+\begin{figure}[htb]
+  \begin{center}
+    \includegraphics[width=0.49\columnwidth]{fig/walk-motion.jpg}
+    \caption{Example of walk pattern generation}
+    \labfig{walk-motion}
+  \end{center}
+\end{figure}
+
+\verb|robot-model|クラスには、オフライン歩行動作生成を行い歩行軌道を返す関数:calc-walk-pattern-from-footstep-listが定義されている。この関数は、1歩ごとの足をつく位置のリストを与えると、ZMPがこの位置に出来る限り近くなるような歩行動作を生成する。以下に示すプログラムは、:calc-walk-pattern-from-footstep-listを用いて歩行動作生成を行うものである(\reffig{walk-motion})。
+
+{\baselineskip=10pt
+\begin{verbatim}
+(load "irteus/demo/sample-robot-model.l")
+(setq *robot* (instance sample-robot :init))
+(send *robot* :reset-pose)
+(send *robot* :fix-leg-to-coords (make-coords))
+(objects (list *robot*))
+
+(let ((footstep-list
+       (list (make-coords :coords (send *robot* :rleg :end-coords :copy-worldcoords) :name :rleg)
+             (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords)
+                                        :translate #f(100 0 0)) :name :lleg)
+             (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords)
+                                        :translate #f(200 0 0)) :name :rleg)
+             (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords)
+                                        :translate #f(300 0 0)) :name :lleg)
+             (make-coords :coords (send (send *robot* :rleg :end-coords :copy-worldcoords)
+                                        :translate #f(400 0 0)) :name :rleg)
+             (make-coords :coords (send (send *robot* :lleg :end-coords :copy-worldcoords)
+                                        :translate #f(400 0 0)) :name :lleg))))
+  (objects (append (list *robot*) footstep-list))
+
+  (send *robot* :calc-walk-pattern-from-footstep-list
+        footstep-list
+        :default-step-height 50
+        :default-step-time 1.0
+        :dt 0.1
+        :debug-view t)
+  )
+\end{verbatim}
+}
+変数\verb|footstep-list|に足をつく位置のリストをセットし、:calc-walk-pattern-from-footstep-listに与えている。\verb|footstep-list|の各要素は、\verb|:name|にいずれの脚かを指定する必要がある。\verb|footstep-list|の最初の要素は初期化のために用いられ、実際にロボットが脚を踏み出すのは次の要素以降の位置に対してである。\verb|:default-step-height 50|では遊脚をサイクロイド補間するときの高さを50[mm]に指定している。\verb|:default-step-time 1.0|は一歩あたりの時間を1.0[s]に指定している。\verb|:dt 0.1|は予見制御のサンプリングタイムと生成される軌道の間隔を0.1[s]に指定している。なお、\verb|:default-step-height|、\verb|:default-step-time|、\verb|:dt|はデフォルト値がこの値となっているため、実際にはこれらの値を設定する必要はない。
+
+
+次に示すプログラムは、移動したい目的地を与えると\verb|footstep-list|を自動で生成して歩行動作生成を行うものである。
+{\baselineskip=10pt
+\begin{verbatim}
+(load "irteus/demo/sample-robot-model.l")
+(setq *robot* (instance sample-robot :init))
+(send *robot* :reset-pose)
+(send *robot* :fix-leg-to-coords (make-coords))
+(objects (list *robot*
+               (apply #'midcoords 0.5 (send *robot* :legs :end-coords))
+               (send (send (apply #'midcoords 0.5 (send *robot* :legs :end-coords))
+                           :translate #F(500 150 0)) :rotate (deg2rad 45) :z)))
+
+(send *robot* :calc-walk-pattern-from-footstep-list
+      (send *robot* :go-pos-params->footstep-list
+            500 150 45) ;; x[mm] y[mm] th[deg]
+      :debug-view t
+      )
+\end{verbatim}
+}
+\verb|footstep-list|として\verb|(send *robot* :go-pos-params->footstep-list 500 150 45)|の返り値を与えて、先の例と同様\verb|:calc-walk-pattern-from-footstep-list|関数で歩行動作を生成している。
+
+\verb|:go-pos-params->footstep-list|関数は、2本足で立ったロボットについて、脚の逆運動学が解ける範囲内で現在の位置から目的地までの\verb|footstep-list|を生成する関数であり、ここでは前に500[mm]、左に150[mm]移動し、左に45[deg]体を回転させるような\verb|footstep-list|を生成している。
+
   \input{irtdyna-func}

doc/irtmodel.tex

@@ -687,7 +687,49 @@
          (ik-thre 1) (ik-rthre (deg2rad 1))
          (q 1.0) (r 1e-6) ;; Q is weighting of output error and R is weighting of input.
          (calc-zmp t))
-   "Calculate walking pattern from foot step list and return pattern list as a list of angle-vector, root-coords, time, and so on."
+   "Calculate walking pattern from foot step list and return pattern list as a list of angle-vector, root-coords, time, and so on.
+    footstep-list should be given.
+    :footstep-list
+    ~ (list footstep1 footstep2 ...). :footstep-list can be any length. Each footstep indicates the destinations of swing legs in each step.
+    ~ footstep should be list of coordinate whose :name is identical with one swing leg and whose coords is the destination of that leg. If number of swing legs in a step is one, the footstep can be a coordinate.
+    ~ footstep1 is only for intialization and not executed.
+    :default-step-height
+    ~ Height of swing leg cycloid trajectories. Default is 50[mm].
+    :dt
+    ~ Sampling time of preview control and output walk pattern. Default is 0.1[s].
+    :default-step-time
+    ~ Reference time of each step. The first 10 percent and the last 10 percent of default-step-time is double support phase. Default is 1.0[s].
+    :solve-angle-vector-args
+    ~ :move-centroid-on-foot is used to solve IK in :calc-walk-pattern-from-footstep-list. :solve-angle-vector-args is passed to :move-centroid-on-foot in the form of (send* self :move-centroid-on-foot ... solve-angle-vector-args). Default is nil.
+    :debug-view
+    ~ Set t to show visualization. Default is nil.
+    :all-limbs
+    ~ list of limb names. In each walking step, limbs in :all-limbs but not assigned as swing legs by :footstep-list are considered to be support legs. Default is '(:rleg :lleg) sorted in force-sensors order.
+    :default-zmp-offsets
+    ~ (list limbname1 offset1 limbname2 offset2 ...). :default-zmp-offsets should include every limb in :all-limbs. offset is a float-vector[mm] and local offset of reference zmp position from end-coords. Default offset is #F(0 0 0)[mm].
+    :init-pose-function
+    ~ A function which initialize robot's pose. Walking pattern is generated from this initial pose. :init-pose-function is called once at the start of walking pattern generation in the form of (funcall init-pose-function). Default is #'(lambda () (send self :move-centroid-on-foot :both '(:rleg :lleg))).
+    :start-with-double-support
+    ~ At the start of walking pattern generation, the initial position of reference zmp is
+    ~~ t: midpoint of all-limbs.
+    ~~ nil: midpoint of swing legs of footstep1.
+    ~ Default is t.
+    :end-with-double-support
+    ~ At the end of walking pattern generation, the final position of reference zmp is
+    ~~ t: midpoint of all-limbs.
+    ~~ nil: midpoint of support legs of the last footstep.
+    ~ Default is t.
+    :ik-thre
+    ~ Threshold for position error to terminate IK iteration. Default is 1[mm].
+    :ik-rthre
+    ~ Threshold for rotation error to terminate IK iteration. Default is (deg2rad 1)[rad].
+    :q
+    ~ Weight Q of the cost function of preview control. Default is 1.0.
+    :r
+    ~ Weight R of the cost function of preview control. Default is 1e-6.
+    :calc-zmp
+    ~ Set t to calculate resultant ZMP after IK. The calculated ZMP is visualized if :debug-view is t, and stored as czmp in return value. Default is t.
+   "
    (let* ((res) (ret) (tm 0)
           (gg (instance gait-generator :init self dt)))
      (funcall init-pose-function)