---
# System prepended metadata

title: 村上メモ

---

$%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}
