Algorithm 1 (Transducer alignment control during the scan).

while fc ≠ fref-c do

if fs > fref then

moveup()

else

movedown()

end if

end while

{Starting the scan with N steps}

for i = 0 : N do

while fc < fref-c – a or fref-c + a < fc do

ve = Kf (fs – fref) {End-effector velocity is defined by the force error}

$\dot{\mathit{q}}$ = J†ve {Transform end-effector velocities to joint velocities}

SendVelocityCommand( $\dot{\mathit{q}}$) {send the velocity commands to the robot}

end while

GetUltrasoundImage() {Save the ultrasound image}

GetTransducerPose() {Save the transducer pose}

MoveTransducer(StepSize) {move the transducer in the scan direction}

end for