Algorithm 1 Calculate the optimal joint position of joint {4}.
Inputs: Position and attitude of {T}, $X_T$, $Z_T$, $R{Z}_T$. Data of attachment, $A_X$, $A_Z$, $A_R$.
Outputs: Position and attitude of {4}, $X_4$, $Z_4$, $R{Z}_4$.
Notes:$\Delta {\mathit{\theta }}_{\mathit{T}}=\left\{0,\cdots ,-\frac{\pi }{6}\right\}$, ${\mathit{\theta }}_{\mathit{e}}=\left\{0,\cdots ,2\pi \right\}$, ${\mathit{\theta }}_{\mathbf{4}}=\left\{\underset{N}{\underbrace{\frac{\pi }{2},\cdots ,\pi }}\right\}$, $\Delta {\mathit{\theta }}_{\mathit{b}}=\left\{-\frac{\pi }{2},\cdots ,0\right\}$.
1.	${\mathit{\theta }}_{\mathit{T}}=\pi -arctan2\left(A_Z,A_X\right)+\Delta {\mathit{\theta }}_{\mathit{T}}$.
2.	Calculate the trajectory of {T} according to Equation (9), and get ${\mathit{x}}_{\mathit{T}}$, ${\mathit{z}}_{\mathit{T}}$.
3.	Calculate the collision region ellipse according to Equation (8), and get ${\mathit{x}}_{\mathit{e}}$, ${\mathit{z}}_{\mathit{e}}$.
4.	Calculate the trajectory of {4} according to Equation (7), and get ${\mathit{x}}_{\mathbf{4}}$, ${\mathit{z}}_{\mathbf{4}}$.
5.	for$i=1,\cdots ,N$
6.	$\hspace{1em}{X}_4=x_4^i$, $Z_4=z_4^i$
7	$\hspace{1em}{\mathit{z}}_{\mathit{W}}={\mathit{z}}_{\mathit{T}}$, calculate ${\mathit{x}}_{\mathit{W}}$, according to Equation (4)
8.	$\hspace{1em}{\mathit{\theta }}_{\mathit{b}}=-\arctan 2\left(Z_4-Z_T,X_4-X_T\right)+\Delta {\mathit{\theta }}_{\mathit{b}}$
9.	Calculate the trajectory of the quick-hitch equipment edge according to Equation (6), and get ${\mathit{x}}_{\mathit{b}}$, ${\mathit{z}}_{\mathit{b}}$.
10.	$\hspace{1em}{d}^i=max\left(\sqrt{{\left({\mathit{x}}_{\mathit{T}}-{\mathit{x}}_{\mathit{W}}\right)}^2+{\left({\mathit{z}}_{\mathit{T}}-{\mathit{z}}_{\mathit{W}}\right)}^2}\right)$.
11.	if arc (${\mathit{x}}_{\mathit{b}}, {\mathit{z}}_{\mathit{b}}$) is tangent to ellipse (${\mathit{x}}_{\mathit{e}}, {\mathit{z}}_{\mathit{e}}$), then
12.	break
13.	end if
14.	end for
15.	$\left[index, d^{min}\right]=min\left(d\right)$
16.	$X_4=x_4^{index}$, $Z_4=z_4^{index}$
17.	$R{Z}_4=arctan2\left(Z_T-Z_4,X_T-X_4\right)-0.236-\frac{\pi }{10}$
18.	Output$X_4$, $Z_4$, $R{Z}_4$.