Setting endpoints for planner using robotCSPace interface is much slower than actually solving the motion plan

[joaomcm]

Hi! In our current project, we have a simple shelf environment and we are using the RobotCSpace and sbl planner to plan our motion. However, we noticed that merely setting the endpoints of for the planner ( i.e. plan.setEndpoints(curr_conf, goal_conf)) is taking upwards of 700ms depending on the machine, while actually finding a solution using our sbl planner once the endpoints are set takes microsseconds. This strikes us as a bit weird. Is the setEndpoints interface doing anything other than verifying the feasibility of the endpoint configurations?

Below is a link with the code and minimum data to reproduce this issue. https://cloud.vi.cs.uni-bonn.de/index.php/s/4WFN6DikP9StT5R

The calls are apparently ending here (https://github.com/krishauser/Klampt/blob/838e05f2959242b5c80dedf55a6e5ed6821f8e57/Python/klampt/plan/motionplanning.py#L873C16-L873C80) , which dives into the low-level CPP library.

Versioning info: Python: 3.9 OS: Ubuntu 22.04 Klampt Versions: 0.9.1 and 0.9.2 (issue is identical in both) , both installed via pip install.

We appreciate any insights into what the problem might be.

Best,

Joao.

[krishauser]

It’s a quirk with the initialization: some planners immediately check the straight line path between the endpoints to see if it already has a feasible path. In this case it looks like the straight line is feasible, and it takes about 200ms to check it at the given resolution / geometry representations. If this was done in the plan call (probably a reasonable change), then your first iteration would take the same time and return a feasible path.

[NilsDengler]

Hi,

I'm not sure if this is the problem: When I check if a path exists right after adding the endpoints, it says that none exists:

plan.setEndpoints(curr_conf, goal_conf)
path = plan.getPath()
print("Path after adding endpoints:", path)
-> Path after enpoint insertion: None

Furthermore, checking the roadmap at this point also shows no edges between these two points. I added the following lines to check your hypothesis:

plan.setEndpoints(curr_conf, goal_conf)
rm = plan.getRoadmap()
print("Roadmap after enpoint insertion:", rm)

-> Roadmap after enpoint insertion: ([[0.0, 1.129785966943347, -1.471786187906031, 2.540078550839643, -1.0682749163925596, 1.130606603250641, 0.0015903135007540989, 0.0, 0.0, 0.0, 0.8000000000000003, 0.0, -0.8025568053451891, 0.0, 0.8025877935073286, 0.802618371470206, 0.803052203378453, 0.0, -0.8026295430606631, 0. 0, 0.0, 0.0], [0.0, 0.918272004726384, -1.042193315555163, 1.690538497255553, -0.648330866114671, 0.9190835932276786, 0.0015919538850657834, 0.0, 0.0, 0.0, 0.8000000000000003, 0.0, -0.8025568053451891, 0.0, 0.8025877935073286, 0.802618371470206, 0.803052203378453, 0.0, -0.8026295430606631, 0.0, 0.0, 0.0]], [])

Also, reducing the collision check resolution from 0.01 to 0.1 results in no time change at all.

Best, Nils

[joaomcm]

Any suggestions on what else can be causing this slowdown? This being independent of the collision checking eps is weird. Our feasibility checks were also rather quick (we validate all possible start and end points - and those colchecks were speedy).

[joaomcm]

I'll close this issue: Your intuition is probably right. Our eps definition variable was just not being set correctly - so eps was set to the default 0.001. It is still weird that the planner found a solution at startup but doesn't report it when probed until you call plan() - but that's an issue for another day.