%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% B. 一般記事
\documentclass[article]{jrsj}
\usepackage{graphicx}
\usepackage{amsmath}\usepackage{amssymb}
\usepackage{algpseudocode}
\usepackage{algorithm}
\usepackage{url}

\renewcommand{\algorithmiccomment}[1]{//#1}
\newcommand{\bm}[1]{\mbox{\boldmath{$#1$}}}
\newcommand{\figlabel}{図\,}
\newcommand{\tablabel}{表\,}

%% 頁（指定する必要なし）
%\setcounter{page}{1}
%% 通巻頁（指定する必要なし）
%\volpage{101}

%% 一般記事の種類を記入（解説，展望，講座など）
\typeofarticle{解説}% 
%% 発行年（コメントアウトしておく）
%\Year{2000}
%% 巻数（コメントアウトしておく）
%\Vol{18}
%% 号数（コメントアウトしておく）
%\No{1}
%% 邦文題名
\title{動作行動開発のための物理エンジンSpringhead}

%% 邦文副題名（ある場合のみ）
\subtitle{}
%% 英文題名
\etitle{Springhead: A physics engine for motion and behavior}
%% 英文副題名（ある場合のみ）
\esubtitle{}
%% 著者のリスト
\authorlist{%
 \authorentry{長谷川 晶一}{Shoichi Hasegawa}{titech}
 \authorentry{三武 裕玄}{Hironori Mitake}{titech}
 \authorentry{田崎 勇一}{Yuichi Tazaki}{nu}
% \authorentry{}{}{}
}
%% 著者の所属
\affiliate[titech]{東京工業大学}{Tokyo Institute of Technology}{神奈川県横浜市緑区長津田町4259}{4259 Nagatsuta-cho, Midori-ku, Yokohama, Kanagawa, Japan }
\affiliate[nu]{名古屋大学}{Nagoya University}{愛知県名古屋市千種区不老町}{Furo-cho, Chikusa-ku, Nagoya, Aichi, Japan}
%% 著者の所属
%\affiliate[label]{和文所属}{英文所属}{和文住所}{英文住所}
%\affiliate[]{}{}{}{}
%% 原稿受付日（記述しない，空欄に）
\received{}
%% 英文キーワード
\keyword{physics engine}

\begin{document}
\maketitle

\section{はじめに}
\label{sec_intro}

ロボットを実用化するためには，様々な環境で所望の機能を果たす動作・行動を作る必要がある．そのような動作・行動の作成は，具体的作業としてはソフトウェアの開発になる．
このようなソフトウェアのテストには，ロボット+動作環境か，そのシミュレータが必要であり，テストができなければ開発を進めることもできない．
実機に比べてコストが低く魅力的なシミュレータだが，その開発にも手間・コストがかかる．

近年デジタルゲーム等に多用される物理エンジンを利用することで，シミュレータ開発の手間を激減させることが期待される．物理エンジンは，手作業で求めた機構の運動方程式の数値計算等で行う通常のシミュレーションとは計算方法が異なり，特有の誤差や不安定性を持つ．このためロボットの動作・行動のテストに汎用物理エンジンを用いる場合には，特別な注意が必要になる．
本稿では，物理エンジンの仕組みと特徴を説明し，ロボットの動作・行動の開発に必要なテスト環境を構築する場合の注意点を指摘する．
更に，筆者らが開発を進めている，動作行動の生成とテストのための物理エンジンSpringheadと動作行動開発事例を紹介する．

\section{物理エンジンとは何か}
\label{sec_overview}

制御やその評価を目的にロボットの動作をシミュレーションする場合，まずラグランジュの運動方程式などその機構の運動を表す運動方程式を立て，次に運動方程式を数値シミュレーションするという手順がしばしば取られる．
これに対して物理エンジンでは，関節等の接続情報にもとづいて運動方程式をその都度物理エンジンが自動的に立て直してシミュレーションする．
このため，関節の追加・削除や接触状態の変化などのような，運動方程式が変化する場合でも，そのままシミュレーションを続けることができる．

物理エンジンでは，運動方程式を自動的に立てる必要があるため，この手間＝計算量も考える必要がある．このため，ラグランジュの運動方程式のような一般座標系の運動方程式でなく，1剛体の運動方程式であるニュートン・オイラーの運動方程式を用い，接触等はこの運動方程式に拘束力を加えると考えることが多い．
\begin{figure}
	\begin{center}
	\includegraphics[width=7cm]{fig/eq_motion.eps}
	\end{center}
\caption{物理エンジンは運動方程式を立てる}
\label{fig:eq_motion}
\end{figure}

\subsection{物理エンジンの処理の流れ}
物理エンジンでは，接触状態や関節での拘束の変化といった拘束条件の変化を反映させるための処理を毎ステップ行う必要がある．具体的には次のような計算をシミュレーションのステップ毎に行う．
\begin{enumerate}
\item 接触判定: 剛体の位置・速度・形状から接触位置や法線の向き等を求める．
\item 拘束条件の更新:接触判定の結果に基づいて，接触による拘束を追加する．また，関節の可動域限界による拘束の追加も行う．
\item 拘束力の計算:拘束条件が与える拘束力を計算する．
\item 運動方程式に基づき速度，位置を更新する．
\end{enumerate}
このため，物理エンジンは運動方程式の立て方と拘束力の計算法により分類できる．

運動方程式の立て方には，
\begin{itemize}
\item 全自由度法：自由運動する6自由度剛体の直交座標系での運動方程式(ニュートン・オイラーの運動方程式)
\item 自由度削減法：関節可動軸など実際に動く自由度についての一般座標系での運動方程式(ラグランジュの運動方程式)
\end{itemize}
がある．全自由度法では，全自由度の位置・速度をシミュレーションする．拘束の変化によって運動方程式の数が変化しないので，接触のように頻繁に変化する拘束にも対応できる．拘束は，拘束力を求めて運動方程式に加えることで実現される．
そのため拘束力の計算に誤差が生じると拘束に誤差が生じる．高速化のため近似計算を用いるため，一部の拘束力が極端に大きな場合などには大きな誤差が残り，結果として関節がずれたり本来曲がらないはずの向きに動いたりしてしまうこともある．チェビシェフリンクのような大きな拘束力が働く機構を用いる場合には注意が必要である．

一方，自由度削減法は，関節が壊れるような回転など拘束が存在する方向には自由度を持たない．このため計算誤差があっても関節がずれるなどといった拘束を破るような誤差は発生しない．また自由度の数＝運動方程式の数が少ないためシミュレーションの計算量が少ない．しかし，関節を外したり追加したりといった拘束に変化が生じた際には運動方程式の数が変化するため処理は多くなる．そのため接触のような頻繁に変化する拘束には向かない．

拘束力の計算法による物理エンジンの分類と各分類に当てはまる論文
\cite{Hahn88}\cite{Mirtich_Thesis}
\cite{Moreau}\cite{Baraff89}\cite{Stewart96}\cite{Moore88}\cite{Yamane}
と物理エンジン\cite{ODE}\cite{Bullet}\cite{Spr2}を
図\ref{fig:taxonomy}に示す．
\begin{figure}
	\begin{center}
	\includegraphics[width=8cm]{fig/taxonomy.eps}
	\end{center}
\caption{拘束の計算法による物理エンジンの分類}
\label{fig:taxonomy}
\end{figure}

\subsection{接触判定手法}
接触判定は，まず接触の可能性がある形状の対を列挙し(Broad phase)，
その後1つの形状の対を細かく調べる(Narrow phase)．
判定結果として得られる情報はNarrow phaseの手法に依存する．

物体形状はポリゴンやプリミティブ等諸々の形式で表現できるが，予め凸形状や凸多角形に分解しておく．局所最小に陥る恐れがないため，距離が極小となる点の対＝最近傍点対となり\cite{GJK}や\cite{LC}のような最近傍点探索アルゴリズムが知られているからである．これらのアルゴリズムは最近傍点対を求めるが，2形状が重なってしまっていると働かない．このため，一旦形状対を侵入が解消する向きにずらしてから判定する等の工夫が必要になる．
接触法線と侵入量は最近傍点対から求める事ができるが，接触面の形状や交差部分の3次元形状を求めるには更に計算が必要になる\cite{HaseEG04}(図\ref{fig:collision})．

\begin{figure}
	\begin{center}
	\includegraphics[width=8cm]{fig/collision.eps}
	\end{center}
\caption{Narrow phaseの接触判定の手順}
\label{fig:collision}
\end{figure}

\subsection{拘束力計算にもとづくシミュレーション手法}
\label{subsec_lcp}
\input{sim_lcp}

\subsection{関節座標系にもとづくシミュレーション手法}
\label{subsec_aba}
\input{sim_aba}

\section{動作行動生成のための物理エンジン}
\label{sec_application}
ロボットの動作や行動の生成では，関節を持つロボットのシミュレーションが多くなる．また，機構だけでなく制御を含めたシミュレーションが重要になる．
本節では，筆者らが開発している物理エンジンSpringhead\cite{Spr2}が持つ，動作行動生成に便利な機能を紹介する．
\input{sim_robot}
\subsection{フィードフォワードのためのトルク計算}
目標軌道を追従する場合，誤差や遅れをなくすには単純なPD制御では高ゲインが必要であり，ゲインを低くするにはフィードフォワードが必要になる．
機構の運動方程式からフィードフォワードに必要なトルクを計算できるが，運動方程式を立てなければならないのは面倒である．本節では，物理エンジンを用いてトルクを求める手法 \cite{TokiSIGGRAPH} を紹介する．

状態変数を保存・再現する機能を持つ物理エンジンでは，世界の状態を保存しておき必要に応じて戻すことができる．これを利用して以下のようにフィードフォワードに必要なトルクを計算しながらシミュレーションを進める．
\begin{enumerate}
\item 現在の状態を保存する． \label{item_save}
\item 制御対象の関節の可動自由度に目標軌道の速度の速度拘束を追加する．可動自由度は無拘束となっているので，ここに$w_i=w_t$($w_t$は目標軌道の速度)の拘束を加える．
\item 接触力が働かないよう設定して，シミュレーション１ステップ進め，拘束力を求める． \label{item_calc}
\item (2)で追加した拘束を元に戻し，(1)の状態に物理エンジンを戻す．
\item (3)で求めたトルクを関節に加える．
\item \label{item_imp}関節にインピーダンスを与えるために，軌道を目標とする低ゲインのPD制御を\ref{subsec_spring_damper}節の手法で行う．
\item シミュレーションを進める．
\end{enumerate}
各関節に(\ref{item_calc})のトルクを与えるので(\ref{item_imp})のPD制御を加えなくても軌道を追従する．外乱への応答は(\ref{item_imp})のPD制御に依る．

\input{sprpyblender}

\section{おわりに}
\input{conclu}

%% 謝辞（ある場合のみ）
%\begin{acknowledgements}
%\end{acknowledgements}
%
%% 文献
\begin{thebibliography}{99}
\input{bib}
\end{thebibliography}

%% 付録（ある場合のみ）
\appendix 

%% 著者紹介（総合論文，学術・技術論文，解説論文のみ）
\begin{biography}
\profile{m}{長谷川 晶一(Shoichi Hasegawa)}{
1974年6月10日生.
1999年東京工業大学大学大学院知能システム科学専攻修士終了．同年ソニー株式会社入社．2000年東京工業大学精密工学研究所助手．2007年電気通信大学知能機械科准教授．2010年東京工業大学精密工学研究所准教授現在に至る．日本バーチャルリアリティ学会，日本ロボット学会，計測自動制御学会，情報処理学会各会員．バーチャルリアリティ，物理エンジン，力触覚，ヒューマンインタフェース，エンタテインメント工学の研究に従事．博士(工学).
}
\profile{m}{三武裕玄(Hironori Mitake)}{
1984年1月12日生.
2011年 東京工業大学大学院知能システム科学専攻博士修了．
2008年4月より2011年3月まで日本学術振興会特別研究員．
2011年より東京工業大学精密工学研究所助教，現在に至る．
日本バーチャルリアリティ学会，日本ロボット学会，情報処理学会各会員．
キャラクタ動作生成，バーチャルリアリティの研究に従事．博士（工学）．
}
\profile{m}{田崎 勇一(Yuichi Tazaki)}{
1980年5月21日生．
2008年3月東京工業大学大学院情報理工学研究科情報環境学専攻博士課程修了．
2007年4月より2009年3月まで日本学術振興会特別研究員．
2008年，Honda Research Institute Europe（ドイツ）客員研究員．
2009年より名古屋大学大学院工学研究科機械理工学専攻助教，今日に至る．
二足歩行ロボットの制御，多自由度ロボットの動作計画などに従事．
計測自動制御学会，IEEE会員．博士(工学)．
}
\end{biography}

\end{document}
