$%2020/06/04
\newcommand\setN[0]{\mathbb{N}}
\newcommand\setZ[0]{\mathbb{Z}}
\newcommand\setQ[0]{\mathbb{Q}}
\newcommand\setR[0]{\mathbb{R}}
\newcommand\setC[0]{\mathbb{C}}
\newcommand\setH[0]{\mathbb{H}}
\newcommand\setE[0]{\mathbb{E}}
\newcommand\pare[1]{{(#1)}}
\newcommand\Pare[1]{\left(#1\right)}
\newcommand\curl[1]{\{#1\}}
\newcommand\Curl[1]{\left\{#1\right\}}
\newcommand\squa[1]{[#1]}
\newcommand\Squa[1]{\left[#1\right]}
\newcommand\abs[1]{\lvert#1\rvert}
\newcommand\Abs[1]{\left\lvert#1\right\rvert}
\newcommand\norm[1]{\|#1\|}
\newcommand\Norm[1]{\left\|#1\right\|}
\newcommand\floor[1]{\lfloor#1\rfloor}
\newcommand\Floor[1]{\left\lfloor#1\right\rfloor}
\newcommand\ceil[1]{\lceil#1\rceil}
\newcommand\Ceil[1]{\left\lceil#1\right\rceil}
\newcommand\angl[1]{\langle#1\rangle}
\newcommand\Angl[1]{\left\langle#1\right\rangle}
\newcommand\conj[1]{\overline{#1}}
\newcommand\transpose[1]{\,{\vphantom{#1}}^t\!#1}
\newcommand\sfrac[2]{#1/#2}
\newcommand\od[2]{\frac{d#1}{d#2}}
\newcommand\pd[2]{\frac{\partial#1}{\partial#2}}
\newcommand\sod[2]{\sfrac{d#1}{d#2}}
\newcommand\spd[2]{\sfrac{\partial#1}{\partial#2}}
\newcommand\tensor[2]{\boldsymbol{\mathrm{T}}^#1_#2}
\newcommand\bm[1]{\boldsymbol{#1}}
\DeclareMathOperator{\tr}{tr}
\DeclareMathOperator{\Re}{Re}
\DeclareMathOperator{\Im}{Im}
\DeclareMathOperator{\Ran}{Ran}
\DeclareMathOperator{\Bnd}{Bnd}
\DeclareMathOperator{\supp}{supp}
\DeclareMathOperator{\esssup}{esssup}
\DeclareMathOperator{\argmin}{argmin}
\DeclareMathOperator{\sgn}{sgn}
\DeclareMathOperator{\Diff}{Diff}
\DeclareMathOperator{\Sym}{Sym}
\DeclareMathOperator{\spn}{span}
\DeclareMathOperator{\grad}{grad}
\DeclareMathOperator{\rot}{rot}
\DeclareMathOperator{\div}{div}
\DeclareMathOperator{\diag}{diag}
\DeclareMathOperator{\rank}{rank}
\DeclareMathOperator{\cov}{cov}
\newcommand\gpow[1]{\lfloor#1\rceil}
\newcommand\tensor[2]{\mathbf{T}^{#1}_{#2}}$

# 村上メモ
以下の運動方程式を考える．
\begin{align}
\ddot{x}=M^{-1}(x)f(x)
\end{align}

正則行列$T$を使い，以下変数$y$を定義する
\begin{align}
y=Tx
\end{align}
$T$は定数の行列であることに注意．

このとき，以下の計算が成り立つ．
\begin{align}
\ddot{y}
&=T\ddot{x}\\
&=TM^{-1}(x)f(x)\\
&=TM^{-1}(T^{-1}y)f(T^{-1}y)\\
&=TM^{-1}(T^{-1}y)T^T(T^T)^{-1}f(T^{-1}y)
\end{align}

ここで，以下で$\tilde{M}$を定義すると，$\tilde{M}$は正定対称．
\begin{align}
\tilde{M}(y)=(TM^{-1}(T^{-1}y)T^T)^{-1}
\end{align}
したがって，
\begin{align}
\ddot{y}
&=\tilde{M}^{-1}(y)(T^T)^{-1}f(T^{-1}y)
\end{align}
が得られた．

# 古賀メモ
$z=[{}^0p_b ,q]^T$として，運動方程式の行列$H$が以下であらわされると仮定する
\begin{align}
H(z)
=
\begin{bmatrix}
H_b & H_{bq}\\
H_{qb} & H_q
\end{bmatrix}
\end{align}
ただし，$H_b,H_q$は正則であると仮定する．
このとき，$H^{-1}$は以下で計算できる．
\begin{align}
H^{-1}
&=
\begin{bmatrix}
H_b^{-1}
+
H_b^{-1}
H_{bq}(H_q-H_{qb}H_b^{-1}H_{bq})^{-1}
H_{qb}
H_b^{-1}
&
-H_b^{-1}
H_{bq}
(H_q-H_{qb}H_b^{-1}H_{bq})^{-1}
\\
-(H_q-H_{qb}H_b^{-1}H_{bq})^{-1}
H_{qb}
H_b^{-1}
&
(H_q-H_{qb}H_b^{-1}H_{bq})^{-1}
\end{bmatrix}\\
&=:
\begin{bmatrix}
A & B\\
C & D
\end{bmatrix}
\end{align}
上記のように定義した行列$A,D$は正則であることに注意しよう．

運動学$f_{0e}:\mathbb{R}^6\to\mathbb{R}^3; z\mapsto f_{0e}(z)$に対し，以下が成立していることに注意する．
\begin{align}
{}^0p_b + T_r({}^0\theta_b)f_{be}(q) = f_{0e}(z)
\end{align}
すると，
\begin{align}
J_{0e}
& :=
\frac{\partial f_{0e}}{\partial z}\\
& =
\begin{bmatrix}
\frac{\partial f_{0e}}{\partial {}^0p_b} &
\frac{\partial f_{0e}}{\partial q}
\end{bmatrix}
\end{align}

このとき，
\begin{align}
W
&:=J_{0e}H^{-1}J_{0e}^T\\
&=
\begin{bmatrix}
\frac{\partial f_{0e}}{\partial {}^0p_b} &
\frac{\partial f_{0e}}{\partial q}
\end{bmatrix}
\begin{bmatrix}
A & B\\
C & D
\end{bmatrix}
\begin{bmatrix}
\frac{\partial f_{0e}}{\partial {}^0p_b}^T \\
\frac{\partial f_{0e}}{\partial q}^T
\end{bmatrix}\\
&=
\begin{bmatrix}
\frac{\partial f_{0e}}{\partial {}^0p_b}A
+\frac{\partial f_{0e}}{\partial q}C
&
\frac{\partial f_{0e}}{\partial {}^0p_b}B
+\frac{\partial f_{0e}}{\partial q}D
\end{bmatrix}
\begin{bmatrix}
\frac{\partial f_{0e}}{\partial {}^0p_b}^T \\
\frac{\partial f_{0e}}{\partial q}^T
\end{bmatrix}\\
&=
\frac{\partial f_{0e}}{\partial {}^0p_b}A
\frac{\partial f_{0e}}{\partial {}^0p_b}^T
+\frac{\partial f_{0e}}{\partial q}C
\frac{\partial f_{0e}}{\partial {}^0p_b}^T
+
\frac{\partial f_{0e}}{\partial {}^0p_b}B
\frac{\partial f_{0e}}{\partial {}^0p_b}^T
+\frac{\partial f_{0e}}{\partial q}D
\frac{\partial f_{0e}}{\partial q}^T
\end{align}

# 岩井メモ
### 定義の確認
\begin{align}
k^a(q,\dot{q})&=\dot{q}^{aT}M^a(q)\dot{q}^a\\
&=\dot{q}^TM(q)\dot{q}
+m_f\dot{q}_f^2
\end{align}

PVFCを適用して外力0なら$\dot{k}^a=0$

PVFC適用して外力ありなら，
\begin{align}
\dot{k}^a=\tau_e^{aT}\dot{q}^a
\end{align}

### 岩井アイディア？
\begin{align}
\dot{q}_f = V_f + \frac{\tau_{c,n+1}^a}{m_f}dt
\end{align}

### 卒論提案法1

正垣PVFCのシミュレーション法を工夫することを提案する

手先にかかる力$\tau_{xy}$を使い，関節にかかる力$\tau_e$は以下で求まる．
\begin{align}
\tau_e=J(q)^T\tau_{xy}
\end{align}
$\tau_e^a$は以下で求める
\begin{align}
\tau_e^a=\begin{bmatrix}
\tau_e\\
0
\end{bmatrix}
\end{align}

シミュレーションは$\dot{q}_f$をシミュレーションせず，かわりに$k^a$をシミュレーションする．
$k^a$のダイナミクスは以下で得られる．

\begin{align}
\dot{k}^a=
\Pare{\tau_c^a+\tau_e^a}^T\dot{q}^a
\end{align}

$\dot{q}_f$は以下で逆算したものを各種制御則の計算の際に利用する．
\begin{align}
\dot{q}_{f}
&=\sqrt{\frac{k^a-\dot{q}^TM(q)\dot{q}}{m_f}}
\end{align}

ただし，以下の計算を使った．
\begin{align}
&\dot{q}^TM(q)\dot{q}
+m_f\dot{q}_f^2=k^a\\
\Rightarrow
&m_f\dot{q}_f^2
=k^a-\dot{q}^TM(q)\dot{q}\\
\Rightarrow
&\dot{q}_f
=\sqrt{\frac{k^a-\dot{q}^TM(q)\dot{q}}{m_f}}
\end{align}

### 卒論提案法2

正垣PVFCのシミュレーション法を工夫することを提案する

手先にかかる力$\tau_{xy}$を使い，関節にかかる力$\tau_e$は以下で求まる．
\begin{align}
\tau_e=J(q)^T\tau_{xy}
\end{align}
$\tau_e^a$は以下で求める
\begin{align}
\tau_e^a=\begin{bmatrix}
\tau_e\\
0
\end{bmatrix}
\end{align}

従来法に加え，$\dot{q}_f$の数値計算誤差用補正項$\tau_i$を加えることを提案する．
すなわち，$\tau=\tau^a_g+\tau^a_r+\tau^a_c+\tau^a_i$という形の制御則を提案する．

$\dot{q}_f$は以下で逆算したものを各種制御則の計算の際に利用する．
\begin{align}
\tau^a_i&=-0.1(\dot{q}_f-\dot{q}_{f,calc})\\
\dot{q}_{f,calc}
&=\sqrt{\frac{k_{calc}^a-\dot{q}^TM(q)\dot{q}}{m_f}}
\end{align}

ただし，ここでの$k^a_{calc}$は以下シミュレーション得られる値のことである．

\begin{align}
\dot{k}_{calc}^a&=\Pare{\tau_c^a+\tau_e^a}^T\dot{q}^a\\
k^a_{calc}(0)&:=k^a(0)
\end{align}

### $\tau_i^a$の雰囲気
\begin{align}
\ddot{q}_f=-0.1(\dot{q}_f-\dot{q}_{f,calc})
\end{align}

\begin{align}
\dot{x}=-0.1(x-x_d)
\end{align}