实时轨迹规划，速度无跳跃

如何解决实时轨迹规划，速度无跳跃

我正在使用OROCOS KDL库为机器人的末端执行器执行实时轨迹规划。每次到达新的目标姿势时，都会在当前姿势和目标姿势之间计划一条新的轨迹，以保持新轨迹和新轨迹之间的速度恒定：

// Trajectory planner function
Trajectory*  newTrajectory_planner(const Frame& target_WP,const Frame& current_pose,double& current_vel,double& current_acc )
{  
    // Geometrical path
    Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.1,new RotationalInterpolation_SingleAxis()); 
    path->Add(current_pose); 
    path->Add(target_WP);
    path->Finish();
    
    // Velocity profile
    VelocityProfile_Spline* velprof = new VelocityProfile_Spline();
    velprof->SetProfileDuration(0,current_vel,current_acc,path->PathLength(),1); 
    Trajectory* traject = new Trajectory_Segment(path,velprof);

    return traject;
}

新轨迹的起始速度等于先前的速度值。这种方法的问题在于，当改变运动方向时，速度会发生跳跃，正如我在此图中所看到的那样（为简化起见，仅在一个轴上）。看到速度突然变化的红色框：

X Axis position,velocity and acceleration

这种行为是可以预期的！我总是采用最后一个速度，所以当它改变方向时，速度的符号也会改变，并且末端执行器不会平稳停止。不论是否使用OROCOS库，如何避免这种情况？