Hi Matt, thanks very much, I haven't seen your answer, sorry, it was months ago haha, at the end I used a different approach, something that wikipedia calls harmonic damped oscillation, or something like that, basically is F = -KX -CV,
I used that formula to drive the position of the rbds to the goal position. Also used a modified version of that formula to make an orientation follow, the result of that research are this two videos.
Thanks very much for your answer by the way.