search

ros-industrial-consortium / descartes

ROS-Industrial Special Project: Cartesian Path Planner
Apache License 2.0
126 stars 92 forks source link

Can't plan more than once #184

Closed BrettHemes closed 7 years ago

BrettHemes commented 7 years ago

@shaun-edwards and @Jmeyer1292

I can't seem to get a DensePlanner instance to plan more than once. On any subsequent calls to planPath, I get errors to the effect of: calculateJointSolutions: IK failed for input trajectory point with ID = ID660624 where the ID is a number much larger than the number of points in my path.

I want to filter the output and then plan again.

Investigating I couldn't even get a second NEW planner object (initialized with the same model as the first) to run...

I figure there is some funny shared pointer action going on somewhere... how do I get everything out of memory? Or rather, has anyone successfully run a single planner twice or two planner back to back?

Jmeyer1292 commented 7 years ago

I will attempt to test and reproduce the issue. We have re-used planners before (e.g. in the ROSCON-2015 demo).

The ID thing is not entirely unexpected. These exist at all because some early applications with Descartes were written with the assumption that the points themselves should have ID's. They are also the handle by which graphs were meant to be mutated.

In your case, all of the internal positions, including every candidate joint solution in the entire graph, are themselves Descartes points with unique ID's. This explains the large numbers.

The IDs (by default at any rate) are uint64_t variables that are generated from a static, mutex-protected variable seen here.

I'll look into it. This is a definite problem.

BrettHemes commented 7 years ago

Ahh, don't look just yet...

I just think I figured it out. I am (naively) sending the Cartesian poses from the planned path back as inputs to the planner for the next iteration without regard to their reference frames (i.e., outputs are tool0 poses wrt world frame while the inputs are (should be) tool_pt poses wrt the tool_base frame).

I will fix this and then post my findings.

Sorry Jonathan

BrettHemes commented 7 years ago

Embarrassing... that was exactly my problem. I transformed the Descartes-returned path back into the tool's base frame and I can then replan no problem.

I will close the issue; thanks.