add documents of calc-static-balance-point with examples

Author: Naoki-Hiraoka

doc/fig/full-body-ik.png

@@ -2017,6 +2017,149 @@ \subsubsection{inverse-kinematicsのtarget-coordsに関数を指定する例}
 このように、拘束条件を踏まえて逆運動学を解きたい場合にはtarget-coordsを関数とし
 て扱うことが必要になる。
 
+\subsubsection{重心位置を考慮したfullbody-inverse-kinematicsの例}
+\begin{figure}[htb]
+  \begin{center}
+    \includegraphics[width=0.49\columnwidth]{fig/full-body-ik.png}
+    \caption{Example of InverseKinematics with root link virtual joint}
+    \labfig{full-body-ik}
+  \end{center}
+\end{figure}
+:fullbody-inverse-kinematicsはロボットの関節に加えてベースリンク仮想ジョイントを駆動した
+逆運動学を解く関数である。以下に示すプログラムは、両足を地面に固定し、重心を両足の上に
+位置させた状態で、左手を目標に到達させる動作を行うものである。
+{\baselineskip=10pt
+\begin{verbatim}
+(load "irteus/demo/sample-robot-model.l")
+(setq *robot* (instance sample-robot :init))
+(send *robot* :reset-pose)
+(setq *obj* (make-cylinder 10 600))
+(send *obj* :rotate pi :x)
+(send *obj* :set-color #f(1 1 0))
+(objects (list *robot* *obj*))
+
+(let* ((rleg-coords (send *robot* :rleg :end-coords :copy-worldcoords))
+       (lleg-coords (send *robot* :lleg :end-coords :copy-worldcoords)))
+   (send *robot* :fullbody-inverse-kinematics
+         (list rleg-coords
+               lleg-coords
+               (make-coords :pos (float-vector 400 100 -600)))
+         :move-target
+         (list (send *robot* :rleg :end-coords)
+               (send *robot* :lleg :end-coords)
+               (send *robot* :larm :end-coords))
+         :link-list
+         (list (send *robot* :link-list (send *robot* :rleg :end-coords :parent))
+               (send *robot* :link-list (send *robot* :lleg :end-coords :parent))
+               (send *robot* :link-list (send *robot* :larm :end-coords :parent)))
+         :translation-axis (list t t t)
+         :rotation-axis (list t t nil)
+         :target-centroid-pos (midpoint 0.5 (send *robot* :rleg :end-coords :worldpos)
+                                            (send *robot* :lleg :end-coords :worldpos))
+         :cog-translation-axis :z)
+      (send *obj* :locate (send *robot* :centroid) :world)
+      (send *irtviewer* :draw-objects))
+\end{verbatim}
+}
+
+{\baselineskip=10pt
+\begin{verbatim}
+(list rleg-coords
+      lleg-coords
+      (make-coords :pos (float-vector 400 100 -600)))
+\end{verbatim}
+}
+の行でtarget-coordsに右足、左足、左手の目標位置姿勢を指定している。
+右足、左足は動かさないため、現在の座標をコピーしたものを与えている。
+このときに、\verb|:translation-axis (list t t t)|, \verb|:rotation-axis (list t t nil)|
+となっているため、右足、左足は位置姿勢を完全に拘束し、左手は姿勢の
+回転は許した条件で逆運動学を解くことになる。
+
+{\baselineskip=10pt
+\begin{verbatim}
+:target-centroid-pos (midpoint 0.5 (send *robot* :rleg :end-coords :worldpos)
+                                   (send *robot* :lleg :end-coords :worldpos))
+:cog-translation-axis :z)
+\end{verbatim}
+}
+の行では重心の逆運動学を指定しており、\verb|:cog-translation-axis :z|で
+z方向の重心の移動は許した状態で\verb|:target-centroid-pos|で目標位置として
+両足の中間の座標を与えることによって、重心のxy座標を両足の中間に一致させる
+条件のもとで逆運動学を解くことになる。
+
+\subsubsection{外力を考慮したfullbody-inverse-kinematicsを解く例}
+\begin{figure}[htb]
+  \begin{center}
+    \includegraphics[width=0.49\columnwidth]{fig/static-balance-ik.png}
+    \caption{Example of InverseKinematics with external force}
+    \labfig{static-balance-ik}
+  \end{center}
+\end{figure}
+ロボットが外力、外モーメントを受ける場合、外力による足裏まわりのモーメントと
+釣り合うようにロボットの重心をオフセットすることによって、バランスをとることができる。
+以下に示すプログラムは両手に外力、外モーメントが加わる場合に、両手両足を目標の位置に
+到達させかつバランスが取れる姿勢を逆運動学によって求めるものである。
+
+{\baselineskip=10pt
+\begin{verbatim}
+(load "irteus/demo/sample-robot-model.l")
+(setq *robot* (instance sample-robot :init))
+(send *robot* :reset-pose)
+(setq *obj* (make-cylinder 10 600))
+(objects (list *robot*))
+
+(let* ((force-list '(#f(-20 0 0) #f(-20 0 0)))
+       (moment-list '(#f(10 0 0) #f(10 0 0))))
+
+  (send *robot* :fullbody-inverse-kinematics
+        (list (send *robot* :rleg :end-coords :copy-worldcoords)
+              (send *robot* :lleg :end-coords :copy-worldcoords)
+              (make-coords :pos #f(400 -300 0))
+              (make-coords :pos #f(400 300 0)))
+        :move-target (mapcar #'(lambda (x)
+                                 (send *robot* x :end-coords))
+                             (list :rleg :lleg :rarm :larm))
+        :link-list (mapcar #'(lambda (x)
+                               (send *robot* :link-list (send *robot* x :end-coords :parent)))
+                           (list :rleg :lleg :rarm :larm))
+        :centroid-offset-func #'(lambda () (send *robot* :calc-static-balance-point
+                                             :force-list force-list
+                                             :moment-list moment-list))
+        :target-centroid-pos (midpoint 0.5 (send *robot* :rleg :end-coords :worldpos)
+                                            (send *robot* :lleg :end-coords :worldpos))
+        :cog-translation-axis :z)
+  (send *irtviewer* :draw-objects)
+
+  ;; draw force
+  (mapcar
+   #'(lambda (f cc)
+       (let* ((prev-color (send *viewer* :viewsurface :color))
+              (prev-width (send *viewer* :viewsurface :line-width)))
+         (send *viewer* :viewsurface :color #F(1 0.3 1))
+         (send *viewer* :viewsurface :line-width 5)
+         (send *irtviewer* :viewer :draw-arrow
+               (send cc :worldpos)
+               (v+ (send cc :worldpos) (scale 10 f)))
+         (send *viewer* :viewsurface :color prev-color)
+         (send *viewer* :viewsurface :line-width prev-width)))
+   force-list
+   (list (send *robot* :rarm :end-coords)
+         (send *robot* :larm :end-coords)))
+  (send *irtviewer* :viewer :viewsurface :flush)
+  )
+\end{verbatim}
+}
+
+この例では、
+{\baselineskip=10pt
+\begin{verbatim}
+:centroid-offset-func #'(lambda () (send *robot* :calc-static-balance-point
+                                     :force-list force-list
+                                     :moment-list moment-list))
+\end{verbatim}
+}
+の行で外力、外モーメントを考慮している。force-listは右手に作用する外力と左手に作用する外力のリスト、force-listは右手に作用する外モーメントと左手に作用する外モーメントのリストであり、単位はそれぞれ[N]、[Nm]である。:calc-static-balance-pointは、現在の両手の位置に作用する外力外モーメントと現在の重心の位置に作用する重力に対してバランスが取れる足裏圧力中心の位置を返す関数である。:centroid-offset-funcは\verb|float-vector|クラスを返す関数を指定することができ、現在の重心位置の代わりにこの関数の返り値を用いて目標重心位置との距離に応じた逆運動学を解く。\verb|:cog-translation-axis :z|でz方向の重心の移動は許した状態で\verb|:target-centroid-pos|で目標重心位置として両足の中間の座標を与えることによって、:centroid-offset-funcの返り値、即ちバランスが取れる足裏圧力中心のxy座標を両足の中間に一致させる逆運動学を解くことになる。
+
  \subsection{ロボットモデル}
 
 ロボットの身体はリンクとジョイントから構成されるが、それぞれ