Right tool for trajectory planning (with non-zero initial velocity)?

edit

I need help to select the right tool (ROS package or even a non-ROS library) for the following goal:

I want to implement a real time trajectory planning for the end effector of a robot. Every time a new target pose arrives (let's call this time an "event"), a new trajectory is planned between the current pose and the target pose, making sure that the initial velocity of the new trajectory is the same as the velocity of the old trajectory at the moment of the event. Therefore, all three velocity components should not change suddenly at the time of the event and the trajectory joint is "smooth".

Using the old OROCOS KDL library, I have implemented an -almost- working simulation, by loading an initial velocity into each new trajectory. The problem with this method is that when the direction of movement is changed, there is a jump in velocity: for example, if there is a translation in the +x direction and then a new target is towards the -x direction, the velocity is reversed from +Vx to -Vx and this produces a sudden change in velocity. The right way would be to desaccelerate until stopping, then accelerate into the new direction. But this case does NOT happen when the new target position is further on the +x direction, as I do not need the robot to stop in that case.

To the trajectory planner experts: how can this be solved? Is there a way to load to the trajectory a velocity profile that always makes sure the Vx, Vy and Vz components are smooth? Is it possible to extend this problem to the 3 axis of movement?