1.在orocos_kinematics_dynamics-master\orocos_kdl\examples\trajectory_example.cpp插入中间路径点总结:
首先调用了Add,内部的流程是首先调用判断是否是(1)起始点(2)中间点(3)指定点(传入的参数)
开始算距离,这里调用Norm简化了欧几里得距离,其实就是根据向量中的最大值进行了化简
接着进行两条线之间的的角度判断,判断是否需要圆弧的过度
接着就是进入直线或者圆弧过度的选择,将中间点放入结构体Path_Line中