Jerkiness When Executing MoveIt Plan After Trajectory Execution
I have a program which executes long trajectories (on the order of 300,000 waypoints), and then once the trajectory is over I typically use the "plan" and "execute" buttons in RViz to move the robot out of the way. However, when I press execute, there is a small delay the robot violently jerks. This often causes a motion supervision error on my robot. I've taken a screenshot of the feedback from the JointTrajectoryController I'm using, and it looks to me like the trajectory is being spliced, resulting in discontinuous velocity, and non-differentiable position goals. I've included screenshots of this.
I'm not sure why this is happening, but my gut is telling me that either it is:
1) Either the controller manager or controller itself is taking so long to parse the trajectory that it misses the first few messages, and picks up at some later point in the trajectory. I suppose I could test this adjusting the timestamps in the moveit msg before it is published. This seems plausible since I'm using a realtime kernel, and perhaps haven't done well in setting up which threads are realtime priority.
2) The very long trajectory never actually finished, and so JointTrajectoryController is trying to splice the trajectories together, and is not limiting jerk in any meaningful way.
Any ideas on which is more likely would be hugely appreciated, the behavior is very startling and seems bad for the robot. Here are the pictures:
