TABLE III:

Detailed MPC algorithm.

1	Initialization: j = 0
(1a)	Set the convergence tolerance εj.
(1b)	Measure $\mathit{q}\left(t_k\right),\dot{\mathit{q}}\left(t_k\right)$.
(1c)	Use virtual constraint and feedback controller to get ${\mathit{h}}_d(\tau ),\overline{\mathit{h}}(\tau )$, and T1(τ), where τ ∊ [tk, tk + TN].
(1d)	Choose initial control trajectory ${\overline{u}}_{fe\mathit{s}}(\tau )\in {\mathcal{U}}_{\left[t_k,t_k+T_N\right]}$ where τ ∊ [tk, tk + TN].
(1e)	Use ${\overline{u}}_{fe\mathit{s}}(\tau )$ and $\overline{\mathit{h}}(\tau )$ to obtain ${\overline{\tau }}_a(\tau )$, therefore, J(j)(tk) where τ ∊ [tk, tk + TN].
2	Optimal Solution Searching:
(2a)	Integrate backwards in time to solve for the costates l(j)(τ) by minimizing the Hamiltonian H = Jmpc + lTΦμ that $\dot{l}(\tau )=-\frac{\partial H\left(\overline{\mu },l,{\overline{u}}_{fe\mathit{s}}\right)}{\partial \overline{\mu }}$.
(2b)	Compute the search direction, a(j)(τ), from the Hamiltonian $a^{(j)}(\tau )=-\frac{\partial H\left(x,l,{\overline{u}}_{fe\mathit{s}}\right)}{\partial {\overline{u}}_{fe\mathit{s}}}$.
(2c)	Compute the optimal step size, σ(j), with adaptive setting in [39].
(2d)	Update the control trajectory
	${\overline{u}}_{fe\mathit{s}}^{(j+1)}(\tau )=\psi ({\overline{u}}_{fe\mathit{s}}^{(j)}+{\sigma }^{(j)}{a}^{(j)})$
	where ψ denotes the constraints.
(2e)	Use ${\overline{u}}_{fe\mathit{s}}^{(j+1)}$ to get J(j+l)(tk).
(2f)	Check Quit Conditions
	(i) quit if $\left|J^{(j+1)}\left(t_k\right)-J^{(j)}\left(t_k\right)\right|\le {\epsilon }_j$,
	(ii) quit if j has exceeded the max iteration limit, Nt,
	(iii) otherwise set j = j + 1 and reiterate gradient step from (2a).