Debugging collision errors

[Jack-ReframeSystems]

I am currently using the built-in FreespacePipeline with a number of Joint waypoints using the Freespace MoveType and the default OMPL configuration setup (seems like RRT with default parameters).

After adding a new collision body attached to the EoAT I am getting the following error message:

Time between points 3 and 4 is not strictly increasing, it is 3.000000 and 3.000000 respectively

This is after I send the trajectory that Tesseract is generating to my simulated robot controller. This causes it to crash.

I understand this most likely is because Tesseract is unable to plan this trajectory due to a collision. I have a couple of questions though:

1. Is this actually a failure according to Tesseract?
2. If this is a failure, how can I check for this case? I am using the TaskComposer directly such as in this example, I saw a similar question on the forum but it seems to be using an API I am not familiar with.
3. How can I debug issues like this in the future? Is there any increased logging or visualization that I can enable? Have others run into this issue and if so, how have you debugged it?

[marip8]

I understand this most likely is because Tesseract is unable to plan this trajectory due to a collision

Not necessarily. The error about the time between points not being strictly increasing seems like something that comes from the time parameterization algorithms, all of which make check that subsequent waypoints happen later in time than previous waypoints.

Have others run into this issue and if so, how have you debugged it?

I have seen something like this before with time parameterization, but I haven't really been able to figure out why it happens. I ended up moving to a different time parameterization algorithm.

Is this actually a failure according to Tesseract?

I would think it should be since the time parameterization algorithm wasn't able to successfully do what it was supposed to

If this is a failure, how can I check for this case? I am using the TaskComposer directly such as in this example, I saw #226 but it seems to be using an API I am not familiar with.

If you are on version 0.18+, you should be able to check TaskComposerInput::isSuccessful after running a task to see if it succeeded or not

How can I debug issues like this in the future? Is there any increased logging or visualization that I can enable? Have others run into this issue and if so, how have you debugged it?

In version 0.18+, you can set TaskComposerInput::dotgraph = true to save a dot graph of the generated task flow with colorization to indicate the tasks that ran/didn't run and succeeded/failed. I think it saves somewhere on /tmp