# Online Foot Location Planning for Gait Transitioning Using Model Predictive Control
## 4 Foot Location Selection Based on MPC
\]  
where \(\left[-\mathbf{f}^{(i)}\right]= \begin{bmatrix}0&\mathbf{f}^{(i)}_{3}&-\mathbf{ f}^{(i)}_{2}\\ -\mathbf{f}^{(i)}_{3}&0&\mathbf{f}^{(i)}_{1}\\ \mathbf{f}^{(i)}_{2}&-\mathbf{f}^{(i)}_{1}&0\end{bmatrix} \) is the GRFs of MPC planning when switching from the swing phase to the support phase in the previous period. The equation of state of the system is  
\[\dot{\mathbf{X}}=\mathbf{A}\mathbf{X}+\mathbf{B}\mathbf{p}_{\mathbf{f}} \tag{15}\]  
\[\mathbf{A}= \begin{bmatrix}0^{3\times 3}&\mathbf{1}^{3\times 3}&\mathbf{0}^{3 \times 3}\\ \mathbf{w}^{3\times 3}&\mathbf{0}^{3\times 3}&\mathbf{0}^{3\times 3}\\ \mathbf{I}^{-1}\Big{[}\sum\limits_{i=1}^{n}\mathbf{\epsilon}(\left|\mathbf{f}^{(i )}\right|)\mathbf{f}^{(i)}\Big{]}&\mathbf{0}^{3\times 3}&\mathbf{0}^{3 \times 3}\end{bmatrix}  \tag{16}\]  
\[\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{-\mathbf{\epsilon}(\left|\mathbf{f}^{(1)}\right|)\cdot\mathbf{w}^{3\times 3}}{2}& \frac{-\mathbf{\epsilon}(\left|\mathbf{f}^{(2)}\right|)\cdot\mathbf{w}^{3\times 3}}{2}& \frac{-\mathbf{\epsilon}(\left|\mathbf{f}^{(3)}\right|)\cdot\mathbf{w}^{3\times 3}}{2}& \frac{-\mathbf{\epsilon}(\left|\mathbf{f}^{(4)}\right|)\cdot\mathbf{w}^{3\times 3}}{2} \\ \mathbf{I}^{-1}\Big{[}-\mathbf{f}^{(1)}\Big{]}&\mathbf{I}^{-1}\Big{[}-\mathbf{ f}^{(2)}\Big{]}&\mathbf{I}^{-1}\Big{[}-\mathbf{f}^{(3)}\Big{]}&\mathbf{I}^{-1} \Big{[}-\mathbf{f}^{(4)}\Big{]}\end{bmatrix}  \tag{17}\]  
\[\mathbf{X}[n+1]=\mathbf{\hat{A}}\mathbf{X}[n]+\mathbf{\hat{B}}\mathbf{p}_{ \mathbf{f}} \tag{18}\]  
Combining the current COM state \(\mathbf{X}_{0}\), the GRFs planned by the MPC when the foot touched the ground in the previous period and the reference state of the robot, the location of the foot is determined according to Equation (18). Let the desired foot location be \(\mathbf{p}_{\mathbf{f}}\). After the foot touches the ground without slipping, the position in the world coordinatesystem does not change. The equation for the change of the state for the next N steps can be obtained. \[ \begin{split}\mathbf{X}_{1}&=\mathbf{\hat{A}}\mathbf{X }_{0}+\mathbf{\hat{B}}\mathbf{p}_{\mathbf{f}}\\ \mathbf{X}_{2}&=\mathbf{\hat{A}}\mathbf{X}_{1}+ \mathbf{\hat{B}}\mathbf{p}_{\mathbf{f}}=\mathbf{\hat{A}}^{2}\mathbf{X}_{0}+ \big{(}\mathbf{\hat{A}}\mathbf{\hat{B}}+\mathbf{\hat{B}}\big{)}\mathbf{p}_{ \mathbf{f}}\\ \mathbf{X}_{3}&=\mathbf{\hat{A}}\mathbf{X}_{2}+ \mathbf{\hat{B}}\mathbf{p}_{\mathbf{f}}=\mathbf{\hat{A}}^{3}\mathbf{X}_{0}+ \big{(}\mathbf{\hat{A}}^{2}\cdot\mathbf{\hat{B}}+\mathbf{\hat{A}}\cdot\mathbf{ \hat{B}}+\mathbf{\hat{B}}\big{)}\mathbf{p}_{\mathbf{f}}\\...\\ \mathbf{X}_{N}&=\mathbf{\hat{A}}\mathbf{X}_{N-1}+ \mathbf{\hat{B}}\mathbf{p}_{\mathbf{f}}=\mathbf{\hat{A}}^{N}\mathbf{X}_{0}+ \big{(}\mathbf{\hat{A}}^{N-1}\cdot\mathbf{\hat{B}}+\mathbf{\hat{A}}^{N-2} \cdot\mathbf{\hat{B}}+\cdot\cdot\cdot+\mathbf{\hat{B}}\big{)}\mathbf{p}_{ \mathbf{f}}=\mathbf{\hat{A}}\mathbf{X}_{0}+\mathbf{\hat{B}}\mathbf{p}_{ \mathbf{f}}\\ \mathbf{\bar{A}}&=\mathbf{\hat{A}}^{N},\mathbf{\bar{B }}=\mathbf{\hat{A}}^{N-1}\cdot\mathbf{\hat{B}}+\mathbf{\hat{A}}^{N-2}\cdot \mathbf{\hat{B}}+\cdot\cdot\cdot+\mathbf{\hat{B}}\end{split}  \tag{19}\]  
The control problem can be transformed into the selection of the foot location that the robot will achieve the desired state at step \(N\). \[\min J=\sum_{i=1}^{N}\Big{(}\mathbf{X}_{(i)}-\mathbf{X}_{(i,ref)}\Big{)}^{ \mathbf{T}}\mathbf{L}\Big{(}\mathbf{X}_{(i)}-\mathbf{X}_{(i,ref)}\Big{)}+( \mathbf{p}_{\mathbf{f}}-\mathbf{p})^{\mathbf{T}}\mathbf{K}(\mathbf{p}_{ \mathbf{f}}-\mathbf{p}) \tag{20}\]  
\[s.\;\text{t.}\;\mathbf{X}[n+1]=\mathbf{\hat{A}}\mathbf{X}[n]+\mathbf{\hat{B}} \mathbf{p}_{\mathbf{f}} \tag{21}\]  
\[\mathbf{p}_{\mathbf{f}}\leq\mathbf{p}_{\mathbf{f}}\leq\mathbf{p}_{\mathbf{f} \mathbf{n}\mathbf{x}} \tag{22}\]  
\[\mathbf{D}\mathbf{p}_{\mathbf{f}}=\mathbf{0} \tag{23}\]  
Both \(\mathbf{L}\) and \(\mathbf{K}\) are adjustable weight matrices and semi-positive definite.