Your pseudocode seems to do what you expect but it is not up to the task at hand since the approach of doing path planning in velocity is fundamentally wrong. Instead, you should be doing path planning in position and motor control in velocity.
When you command a robot in velocity without taking into account the corrections required to compensate for even small deviations from the intended position path (this mode is called open-loop), then the robot inevitably ends up in a location different from $X^R_{goal}$. This happens both in a simulation where the integration of velocity cannot be perfect and, more importantly, with a real robot where impairments, noise, and unmodeled factors contribute to significant deviations from the ideal behavior.
To get around this, the simplest solution would be to implement a closed-loop velocity control over the planned position path. In particular, the controller can be a PI, that is a proportional-integral unit.
In essence, suppose we have a vector $traj$ containing position waypoints connecting $X^R_{init}$ and $X^R_{goal}$. For each dimension of $traj$ at each control instant $t=t_i$, you should compute the velocity command as:
$$
v[t_i] = K_P \cdot e[t_i] + K_I \cdot \sum_{t=t_0}^{t=t_i} e[t],
$$
where $e[t_i]=traj[t_i]-loc[t_i]$ represents the instantaneous error between the desired path and the actual location (i.e. the feedback) at time $t=t_i$. Further, $K_P$ and $K_I$ are the so-called proportional and integral gains of the PI controller that regulate the contributions of the corrections induced by the actual error and the error accumulated over time, respectively.
There are diverse methodologies to tune the gains, but you could easily come up with working values by setting up initially $K_I=0$ and slightly increase $K_P$ throughout subsequent sessions until you will get a decent tracking. The final displacement error can be compensated by playing with $K_I$.
Finally, you can even think of mixing open-loop and closed-loop modalities. In this context, the velocity command will resemble something like the following:
$$
v[t_i] = v_{OL}[t_i] + K_P \cdot e[t_i] + K_I \cdot \sum_{t=t_0}^{t=t_i} e[t],
$$
where $v_{OL}[t_i]$ is the open-loop vector (the so-called feed-forward term) that can be populated using your pseudocode. The gains $K_P$ and $K_I$ will assume different values compared to the case of a pure feedback controller. You will experience anyway that without the feedback term the robot won't attain the goal.