Invalid Trajectory: start point deviates from current robot state more than 0.01

[rkeatin3]

I'm experiencing an issue in which trajectory execution fails with the error Invalid Trajectory: start point deviates from current robot state more than 0.01. I have experienced this both when running my robot in simulation and on the real hardware. I've written a Python script that plans and executes task-space plans in rapid succession, and that seems to be the key to repeatably recreating this issue.

A couple things of note: 

1. Adding a sleep of 0.5 second after waiting for execution to finish seems to totally mitigate this issue. 
2. My print statements for joint values (both from the joint_states topic and the MoveGroup API) differ from those printed along with the error. See a few below that I recorded in simulation (with the errors from the move_group node in red and print statements from my script in blue).

Invalid Trajectory: start point deviates from current robot state more than 0.01

joint 'pre_oven_elbow_joint': expected: -1.52124, current: -1.53823

Pre-Planning: 

joint_state values:  [-1.0802693709443139, -0.37424761714342125, -1.5375689959284644, 0.3402320839261135, 1.5712472784252647, -1.275428323998037]

move_group values:  [-1.0802702970022464, -0.37381138016719184, -1.538226709236505, 0.34045307660232904, 1.571246065687224, -1.2754273338705966]

Post-Planning: 

joint_state values:  [-1.0802702970022464, -0.37381138016719184, -1.538226709236505, 0.34045307660232904, 1.571246065687224, -1.2754273338705966]

move_group values:  [-1.0802702970022464, -0.37381138016719184, -1.538226709236505, 0.34045307660232904, 1.571246065687224, -1.2754273338705966]

On Execution: 

joint_state values:  [-1.0802702970022464, -0.37381138016719184, -1.538226709236505, 0.34045307660232904, 1.571246065687224, -1.2754273338705966]

move_group values:  [-1.0802702970022464, -0.37381138016719184, -1.538226709236505, 0.34045307660232904, 1.571246065687224, -1.2754273338705966]

Invalid Trajectory: start point deviates from current robot state more than 0.01

joint 'pre_oven_shoulder_pan_joint': expected: -1.10079, current: -1.08082

Pre-Planning: 

joint_state values:  [-1.0808224789412524, -0.3745636464009445, -1.5384563926793042, 0.3429131951007136, 1.5705094606638736, -1.2756881253600785]

move_group values:  [-1.0808224789412524, -0.3745636464009445, -1.5384563926793042, 0.3429131951007136, 1.5705094606638736, -1.2756881253600785]

Post-Planning: 

joint_state values:  [-1.0808224743504216, -0.3745636462383928, -1.538456392694818, 0.3429131950755595, 1.5705094606377683, -1.2756881253743053]

move_group values:  [-1.0808224743504216, -0.3745636462383928, -1.538456392694818, 0.3429131950755595, 1.5705094606377683, -1.2756881253743053]

On Execution: 

joint_state values:  [-1.0808224743504216, -0.3745636462383928, -1.538456392694818, 0.3429131950755595, 1.5705094606377683, -1.2756881253743053]

move_group values:  [-1.0808224743504216, -0.3745636462383928, -1.538456392694818, 0.3429131950755595, 1.5705094606377683, -1.2756881253743053]

Invalid Trajectory: start point deviates from current robot state more than 0.01

joint 'pre_oven_elbow_joint': expected: -1.52112, current: -1.53807

Pre-Planning: 

joint_state values:  [-1.0811257451530167, -0.3745603655774534, -1.537394402570115, 0.34044304459786723, 1.5705153052978051, -1.2744215254927127]

move_group values:  [-1.0811257451530167, -0.3745603655774534, -1.537394402570115, 0.34044304459786723, 1.5705153052978051, -1.2744215254927127]

Post-Planning: 

joint_state values:  [-1.0811268057862202, -0.3741136239421028, -1.5380676385337129, 0.3406692847224173, 1.5705145402437708, -1.274420304171361]

move_group values:  [-1.0811268057862202, -0.3741136239421028, -1.5380676385337129, 0.3406692847224173, 1.5705145402437708, -1.274420304171361]

On Execution: 

joint_state values:  [-1.0811268057862202, -0.3741136239421028, -1.5380676385337129, 0.3406692847224173, 1.5705145402437708, -1.274420304171361]

move_group values:  [-1.0811268057862202, -0.3741136239421028, -1.5380676385337129, 0.3406692847224173, 1.5705145402437708, -1.274420304171361]

Is it possible that the "current" state of the robot isn't being update fast enough for motions executed in quick succession? I'm also surprised to see that, like in those I've bolded, the value sometimes change between initiating the planning and when the planning has been finished (when the robot should be static). Though small at 6E-4, the noise in simulation seems to be around 1E-10. Could the previous trajectory execution be returning before the robot has actually stopped moving? 

Thoughts? 

[fluri...@gmail.com]

Hi rkeatin3,

could you post the code related to the path planning?
It could have something to do in how you fill the path with waypoints, or the timing of your arm.go() call.