Control law with PIV and trajectory generation