Sort of solved a bug in the p2p-trajectory. Solved the IK problem (numerical drift problem). Got successful planning results. Still buggy.