Revision history [back]

I recently started testing sbpl_lattice_planner and also had this constant 45 degree error. And as mentioned, it was also ~50% of the time. Originally I thought it was the discretization error that mphillips mentioned, until I updated sbpl_lattice_planner.cpp to display the planned orientation (including position). Turns out that it wasn't a discretization problem, but an actual planning problem. On occasions that there was the 45 degree error, the planner did, in fact, plan the final pose with that error.

To cut the story short, the error occurs in primID: 6, startangle_c: 15, endpose_c: 0 0 0 of pr2.mprim. The planner puts all the intermediate poses into the plan EXCEPT for the end pose, so in this case, the last pose in the plan has angle 0.6545rad (37.5deg). This is the heading error that we are seeing and occurs when the goal pose has 0 deg orientation.

So the simple solution is to change this 0.6545 to 0.000. Take note that for all other primitives, the end pose will not be pushed into the plan as well, but because the last intermediate pose is close to the end pose, you won't notice the difference (1/9th of the resolution). It is only this particular primID, startangle, endpose where the difference is obvious.