# 式
## 躍度最小軌道
### 位置
$$MinJerk(t)=\begin{cases}
(t<t_0)&x_0\\
(t>t_f)&x_f\\
(t_0\le t\le t_f)&x_0+(x_f-x_0)(6(\frac{t-t_0}{t_f-t_0})^5-15(\frac{t-t_0}{t_f-t_0})^4+10(\frac{t-t_0}{t_f-t_0})^3)
\end{cases}
$$
#### 合成
$$\boldsymbol{x_i}=\begin{cases}
(i=0)&\boldsymbol{x_0}\\
(i\neq0)& \displaystyle \boldsymbol{x_0}+\sum^j\boldsymbol{MinJerk_j}(i)&(\text{jはj番目の躍度最小軌道})
\end{cases}$$

### 速度
$$MinJerk_v(t)=\begin{cases}
(t<t_0||t>t_f)&0\\
(t_0\le t\le t_f)&\frac{(x_f-x_0)}{(t_f-t_0)}(30(\frac{t-t_0}{t_f-t_0})^4-60(\frac{t-t_0}{t_f-t_0})^3+30(\frac{t-t_0}{t_f-t_0})^2)
\end{cases}
$$
#### 合成
$$\boldsymbol{x_i}=\begin{cases}
(i=0)&\boldsymbol{0}\\
(i\neq0)& \displaystyle \sum^j\boldsymbol{MinJerk_j}(i)&(\text{jはj番目の躍度最小軌道})
\end{cases}$$

### 角度
$\boldsymbol{MinJerk_{q}}(t)=Slerp(r(t),\boldsymbol {q_0},\boldsymbol{q_f})$
$$r(t)=\begin{cases}(t<t_0)&0\\
(t_0\le t\le t_f)&6(\frac{t-t_0}{t_f-t_0})^5-15(\frac{t-t_0}{t_f-t_0})^4+10(\frac{t-t_0}{t_f-t_0})^3\\
(t_f\le t)&1\end{cases}$$

#### 合成
- $\boldsymbol q$はQuaternion

$$\boldsymbol {q_i}=
[\displaystyle \prod_jSlerp(r_j(i),\boldsymbol {{q_{j0}}},\boldsymbol {q_{jf}})]\boldsymbol {q_0}\:\:\:\:(\text{jはj番目の躍度最小軌道})
$$

- i番目の目標姿勢 $\theta_i$
- $\Delta\theta_i = \theta_{i}\theta_{i-1}^{-1}$
- $\displaystyle\theta(t) =[ \prod_{j=1}^N Slerp(r(t_s^j, t_f^j, t), (1,0,0,0), \Delta\theta_j)] \theta_0$

- $t < t_s^0のとき、\theta(t) = \theta_0$
- $t > t_f^Nのとき、\theta(t) = [\theta_N \theta_{N-1}^{-1}] [\theta_{N-1} \theta_{N-2}] ... [\theta_1 \theta_0^{-1}] \theta_0 = \theta_N$
$||2\times\dot q||=||q\omega||=||\omega||$
$\displaystyle\int\sqrt{({\frac{dx}{dt}})^2+({\frac{dy}{dt}})^2+({\frac{dz}{dt}})^2}dt$

$W=\cos(\frac{\sqrt{x^2+y^2+z^2}}{2})$

次にやってみる方法：以下の式で各目標時刻をもとめてみる
$min [ \int|\frac{d\theta_{real}(t)}{dt}|dt - \int|\frac{d\theta_{estimate}(t)}{dt}|dt ]$
- $\theta_{real}$はQuaternion
- $\theta_{estimate}$は1次元
- $\int|\frac{d\theta_{real}(t)}{dt}|dt$の部分

//ori_dataは元データ(Quaternion)
//dif_dataは差分の二乗和のsqrt
//rot_dataはdif_dataの和
rot_data[0]=0;
for (int i = 0; i < size - 1; i++) {
    dif_data[i] = pow(pow(ori_data[i + 1].x - ori_data[i].x, 2)+ pow(ori_data[i + 1].y - ori_data[i].y, 2)+ pow(ori_data[i + 1].z - ori_data[i].z, 2)+ pow(ori_data[i + 1].w - ori_data[i].w, 2),0.5);
    rot_data[i + 1] = rot_data[i] + dif_data[i];
}

- $\int|\frac{d\theta_{real}(t)}{dt}|dt$の部分
$\int|\frac{d\theta_{real}(t)}{dt}|dt=|\theta_{real}(t)|$とした。

評価関数はこれらの二乗誤差+始めと終わりが一致するように重み付け+時間があまり範囲外に行かないようにするためのペナルティー
それぞれグラフにして$t_{s0}\to t_{s2}$と$t_{f0}\to t_{f2}$をプロット

t0,tfを概算し、変動させながら推定する

目標位置だけでなく目標速度を指定する場面があるかもしれない

## Slerp
$\Delta \boldsymbol {q}=\boldsymbol {q_2}\boldsymbol {q_1}^{-1}$
$\displaystyle\phi=\frac\theta2=\arccos(\Delta \boldsymbol q.w)$
$\displaystyle \Delta \boldsymbol{q^t}=\frac{\sin(1-t)\phi}{\sin\phi}\Delta\boldsymbol {q_0}+\frac{\sin t\phi}{\sin\phi}\Delta\boldsymbol {q_1}$
$$Slerp(t,\boldsymbol {q_1},\boldsymbol {q_2})=\Delta \boldsymbol q^t\boldsymbol{ q_1}=\frac{\sin(1-t)\phi}{\sin\phi}\boldsymbol {q_1}+\frac{\sin t\phi}{\sin\phi}\boldsymbol {q_2} \:\:\:\: (0\le t\le1)$$

## 最小二乗法
### 位置
$ls=\displaystyle\sum_j(x_j-x_{(original)j})^2$

### 角度
回転ベクトル(x,y,z)
$\frac\theta2=\frac{\sqrt{x^2+y^2+z^2}}{2}$
$w=\cos(\theta/2)$
$x=\lambda_x\sin(\theta/2)$
$y=\lambda_y\sin(\theta/2)$
$z=\lambda_z\sin(\theta/2)$

$ls=\displaystyle\sum_j(\arccos((q_j^{-1}q_{(original)j}).W))^2$
↑の式は$q_jからq_{original j}$への回転を表す$QuaternionのW$
$x^2+y^2+z^2+w^2=1$
## Quaternion
### 積
$$qp=\begin{cases}x:q_wp_x−q_zp_y+q_yp_z+q_xp_w\\y:q_zp_x+q_wp_y−q_xp_z+q_yp_w\\z:−q_yp_x+q_xp_y+q_wp_z+q_zp_w\\w:−q_xp_x−q_yp_y−q_zp_z+q_wp_w\end{cases}$$
### 逆回転
$$q^{-1}=(-x,-y,-z,w)$$
### 微分
$$\dot q=\frac12q\omega$$

## 回転ベクトルとQuaternion.W
回転ベクトル$(x,y,z)$
$W=\cos(\frac{\sqrt{x^2+y^2+z^2}}{2})$
$\theta=2*\arccos(W)$
$\theta\neq\sqrt{x^2+y^2+z^2}$

## 回転ベクトルToQuaternion

## QuaternionTo回転ベクトル

$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$
$\:$

---
:::spoiler ---
$f_q(t)=(t<0)?\text{identity}:(t>1)?q:slerp(6t^5-15t^4+10t^3,i,q)・・・(iは無回転、qは目的地)$
$f_q'(t)=(0\le t\le 1)?slerp'(6t^5-15t^4+10t^3,i,q):\boldsymbol0$
$T_1=6t^5-15t^4+10t^3$
$T_2=30t^4-60t^3+30t^2$
$slerp'(t,i,q)=(-iT_2\theta\cos((1-T_1)\theta)+qT_2\theta\cos(T_1\theta))/sin\theta$

todo
- 頭や手のデータを取って躍度最小軌道に分解し、Vgentで動かしてみる(まずは手だけ)
- 長時間動作はいくつかの窓に分割してできるか考える
- Survey(関連研究を探す)
- 最適化部分をdll化してC#から呼び出す
- Springheadのマルチスレッドで高速化もできる