Algorithm 1 Move the tip of the catheter following via points
1:	procedure CATHETERMOVE(X,θ0)
2:	k ← 1
3:	${\overline{x}}_k\leftarrow p({\theta }_0),x_g\leftarrow \text{first}x\in X$
4:	repeat
5:	Determine ${\dot{x}}^{\ast }$ from (10)
6:	Solve (8), (4), (5) with $\dot{x}={\dot{x}}^{\ast }$ for u*
7:	uk ← u*
8:	Apply uk to (3) for ∆t to obtain xk
9:	if Reached(xk +1,xg) then
10:	xg ← next x ∈ X
11:	end if
12:	k ← k + 1
13:	until All x ∈ X are visited
14:	return (xk, uk), ∀k
15:	end procedure