# Online Foot Location Planning for Gait Transitioning Using Model Predictive Control
## 4 Foot Location Selection Based on MPC
Extending the COM state to \(\mathbf{x}=[\mathbf{p},\dot{\mathbf{p}}]^{\mathbf{T}}\), \(\mathbf{p}\in\mathbf{R}^{3\times 1}\) is the position of the COM in the world coordinate system. \(\dot{\mathbf{p}}\in\mathbf{R}^{3\times 1}\) is the velocity of the COM in the world coordinate system. The position of the supporting foot is extended to the coordinates of the foot in the world coordinate system during the motion of the quadruped robot. The equation of state is shown in Equation (6). \[\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{p_{f}} \tag{6}\]  
\[\mathbf{A}= \begin{bmatrix}\mathbf{0}^{3\times 3}&\mathbf{1}^{3\times 3}\\ \mathbf{w}^{3\times 3}&\mathbf{0}^{3\times 3}\end{bmatrix}  \tag{7}\]  
\[\mathbf{B}= \begin{bmatrix}\mathbf{0}^{3\times 3}&\mathbf{0}^{3\times 3}& \mathbf{0}^{3\times 3}&\mathbf{0}^{3\times 3}&\\ \frac{-\epsilon(|\mathbf{f}^{(1)}|)\cdot\mathbf{w}^{3\times 3}}{2}&\frac{- \epsilon(|\mathbf{f}^{(2)}|)\cdot\mathbf{w}^{3\times 3}}{2}&\frac{- \epsilon(|\mathbf{f}^{(3)}|)\cdot\mathbf{w}^{3\times 3}}{2}&\frac{- \epsilon(|\mathbf{f}^{(4)}|)\cdot\mathbf{w}^{3\times 3}}{2}\end{bmatrix}  \tag{8}\]  
\[\mathbf{w}^{3\times 3}= \begin{bmatrix}g/z_{0}&0&0\\ 0&g/z_{0}&0\\ 0&0&0\end{bmatrix}  \tag{9}\]  
\[\epsilon\Big{(}|\mathbf{f}^{(i)}|\Big{)}= \begin{cases}0&\left|\mathbf{f}^{(i) }\right|>0\\ 1&\left|\mathbf{f}^{(i)}\right|=0\end{cases}  \tag{10}\]  
where \(\mathbf{p_{f}}= \begin{bmatrix}\mathbf{p_{f}}^{(1)}&\mathbf{p_{f}}^{(2)}& \mathbf{p_{f}}^{(3)}&\mathbf{p_{f}}^{(4)}\end{bmatrix} ^{\mathbf{T}}\)is the position of the foot in the world coordinate system. \(\mathbf{p_{f}}^{(i)}= \begin{bmatrix}p_{f1}^{(i)}&p_{f2}^{(i)}&p_{f3}^{(i)} \end{bmatrix} ^{\mathbf{T}}\in\mathbf{R}^{3\times 1}\) and \(\mathbf{f}^{(i)}= \begin{bmatrix}f_{1}^{(i)}&f_{2}^{(i)}&f_{3}^{(i)}\end{bmatrix}  \in\mathbf{R}^{3\times 1}\) are the coordinates of the foot i and the GRFs in the world coordinate system, respectively. In the LIP model, the choice of the foot location affects the velocity of the COM, but not the height of the COM. Equation (10) is used to determine the swing leg. The presence of GRFs makes the location of the foot affect the COM posture. Therefore, the choice of foot location needs to consider the effect on the velocity and the posture. The dynamics of a quadruped robot rotating around the COM in the world coordinate system is shown in Equation (11). \[\frac{\mathrm{d}}{\mathrm{dt}}(\mathbf{I}\mathbf{\omega})=\sum\limits_{i=1}^{n} \mathbf{r}^{(i)}\times\mathbf{f}^{(i)} \tag{11}\]  
where \(\mathbf{I}\) is the robot's inertia tensor, \(\mathbf{\omega}\in\mathbf{R}^{3\times 1}\) is the angular velocity of the robot. \(\mathbf{r}\) is the vector between the position of the foot end and the COM. When \(\mathbf{\omega}\) is small, the left side of Equation (11) can be approximated with:  
\[\frac{\mathrm{d}}{\mathrm{dt}}(\mathbf{I}\mathbf{\omega})=\mathbf{I}\dot{\mathbf{ \omega}}+\mathbf{\omega}\times(\mathbf{I}\mathbf{\omega})\approx\mathbf{I}\dot{\mathbf{ \omega}} \tag{12}\]  
Converting the right-hand side of Equation (11) to a representation of the COM position and the foot position in the world coordinate system. \[\sum\limits_{i=1}^{n}\mathbf{r}^{(i)}\times\mathbf{f}^{(i)}=\sum\limits_{i=1} ^{n}\left(\mathbf{p}\mathbf{r}^{(i)}-\mathbf{p}\right)\times\mathbf{f}^{(i)}= \sum\limits_{i=1}^{n}\mathbf{f}^{(i)}\times\left(\mathbf{p}-\mathbf{p}\mathbf{ r}^{(i)}\right) \tag{13}\]  
Then Equation (11) can be converted to:  
\[\dot{\mathbf{\omega}}= \left[\mathbf{I}^{-1}\Big{[}\sum\limits_{i=1}^{n}\mathbf{\epsilon}( \left|\mathbf{f}^{(i)}\right|)\mathbf{f}^{(i)}\Big{]}\quad 0\right]\cdot \begin{bmatrix} \mathbf{p}\\ \mathbf{\omega}\end{bmatrix}  \tag{14}\] \[+\left[\mathbf{I}^{-1}\Big{[}-\mathbf{f}^{(1)}\Big{]}\quad \mathbf{I}^{-1}\Big{[}-\mathbf{f}^{(2)}\Big{]}\quad\mathbf{I}^{-1}\Big{[}- \mathbf{f}^{(3)}\Big{]}\quad\mathbf{I}^{-1}\Big{[}-\mathbf{f}^{(4)}\Big{]} \right] \begin{bmatrix}\mathbf{p}\mathbf{r}^{(1)}\\ \mathbf{p}\mathbf{r}^{(3)}\\ \mathbf{p}\mathbf{r}^{(4)}\end{bmatrix}