Ｗhole body kinematics
===


## Torque控制(for stance legs)
**Note:以下粗體均為向量**

$$\underbrace{
\begin{bmatrix}
\mathbf{I}_3 & \dots & \mathbf{I}_3 \\
[p_1 - p_c]_\times & \dots & [p_4 - p_c]_\times
\end{bmatrix}
}_{A}
\mathbf{F}_{\text{leg}} =
\underbrace{
\begin{bmatrix}
m (\ddot{\mathbf{p}}_{c,d}+ \mathbf{g}) \\
\mathbf{I}_G \dot{\boldsymbol{\omega}}_{b,d}
\end{bmatrix}
}_{b},\quad (1)$$
- 其中$b$項中的$m\mathbf{g}$為feedforward項，而$m\ddot{\mathbf{p}}_{c,d}$為feedback項，定義如下
$$
\begin{bmatrix}
\ddot{\mathbf{p}}_{c,d} \\
\dot{\boldsymbol{\omega}}_{b,d}
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{K}_{p,p} (\mathbf{p}_{c,d} - \mathbf{p}_c) + \mathbf{K}_{d,p} (\dot{\mathbf{p}}_{c,d} - \dot{\mathbf{p}}_c) \\
\mathbf{K}_{p,\omega} e_R + \mathbf{K}_{d,\omega} (\boldsymbol{\omega}_{b,d} - \boldsymbol{\omega})
\end{bmatrix}\quad (2)
$$
$\mathbf{p}_c$為質心位置，$\mathbf{p}_{c,d}$為理想的質心位置(可參考此[連結](https://hackmd.io/IUP1grLtSdOwgh3heqkmsQ))定義如下：
$$\mathbf{p}_{c,d} = [ ^I r_{B_x}^{d}, ^I r_{B_y}^{d}, h_g + h_H(\phi), 0, 0, 0]^\mathbf{T},$$$$\mathbf{\dot{p}}_{c,d} = [ v_{B_x}^{d}, v_{B_y}^{d}, 0, 0, 0, \dot{\psi}_d]^\mathbf{T}$$
$\boldsymbol{\omega}$為角速度，$\boldsymbol{\omega}_{b,d}$為身體的理想角速度，$\mathbf{F}_{\text{leg}}$為各隻腳確保穩定性所需的作用力(從地面提供，又稱地面反作用力，包含正向力+摩擦力)，$\mathbf{e_R}$定義為$$\mathbf{e_R}= -\frac{1}{2} \left( R_d^T R - R^T R_d \right)^{\vee},$$$R$、$R_d$分別為身體姿態、理想身體姿態。再透過Jacobian matrix，可轉換出的每軸所需力矩。$$\mathbb{\tau} = \mathbf{J}^T \mathbf{F}_{\text{leg}}\quad (3),$$where
$$\mathbf{F}_{\text{leg}}=[\mathbf{F}_{\text{leg,1}},\mathbf{F}_{\text{leg,2}},...,\mathbf{F}_{\text{leg,n}}],$$where $n$ is the number of stance legs. Vee map ($\vee$) is **inverse of skew operator** defined by the relationship $\overset{\vee}{M}=V,$where $$V=\begin{bmatrix} x \\ y \\ z \end{bmatrix}$$,
$$
M = \begin{bmatrix}
0 & -z & y \\
z & 0 & -x \\
-y & x & 0
\end{bmatrix}$$

## 靜態補償力(重力補償)
如果前述公式所有微分像都為零，可算出靜態下讓機器人穩定條件下，每隻腳所需的力。
$$\underbrace{
\begin{bmatrix}
\mathbf{I}_3 & \dots & \mathbf{I}_3 \\
[p_1 - p_c]_\times & \dots & [p_4 - p_c]_\times
\end{bmatrix}
}_{A}
\mathbf{F}_{\text{leg}} =
\underbrace{
\begin{bmatrix}
\mathbf{mg+m\mathbf{K}_{p,p} (\mathbf{p}_{c,d} - \mathbf{p}_c)} \\
\mathbf{K}_{p,\omega} e_R
\end{bmatrix}
}_{b}\quad(4),$$
並可接著算出每軸所需力矩為$$\mathbb{\tau} = \mathbf{J}^T \mathbf{F}_{\text{leg}},\quad (5)$$

## 前饋力矩(feedforward重力補償)
如果前述公式只考慮重力，可算出讓機器人穩定條件下，每隻腳所需的力和每軸的力矩。
$$\underbrace{
\begin{bmatrix}
\mathbf{I}_3 & \dots & \mathbf{I}_3 \\
[p_1 - p_c]_\times & \dots & [p_4 - p_c]_\times
\end{bmatrix}
}_{A}
\mathbf{F}_{\text{leg}} =
\underbrace{
\begin{bmatrix}
\mathbf{mg} \\
0
\end{bmatrix}
}_{b}\quad,$$
並可接著算出每軸所需力矩為$$\mathbb{\tau_f} = \mathbf{J}^T \mathbf{F}_{\text{leg}},\quad $$$$
= \mathbf{J}^T (A^{\dagger}b),\quad
$$