PlanToConfig doesn't check if robot is already at the config

[DavidB-CMU]

prpy.PlanToConfig() doesn't seem to check if the arm is already at the config that you want to go to.

There may also be a problem with wrapping of angles e.g. not realizing a current joint angle matches an angle in the config you want to go too.

[mkoval]

There isn't any single place where we could put this logic because each planner provides an independent implementation of PlanToConfiguration. Even if it were possible, I am not sure that this is a good idea: most planners naturally cope with this case, i.e. by immediately returning success, without the neeed for an additional check.

May I ask what unexpected behavior you encountered that prompted this issue?

[psigen]

@mkoval @DavidB-CMU: I actually encountered a very related issue.

I was planning to a TSR just on the surface of a table. The TSR bounds in Z had enough slop to go through the table a bit...

What I got was a trajectory that was half a DOF resolution into collision at its goal, which meant that the VanDerCorputGenerator did not output the final goal configuration, instead checking a previous point which was out of collision a half-DOF-resolution step away.

However, when I attempted to plan another trajectory that started at the end of this one, the new plan was now checked exactly at its start (the end of the previous trajectory) which was in collision even though the previous planner returned success. This led to the subsequent planning call always failing (and my discovery of bug #298).

This makes me think we should always check the start and goal endpoints' constraints explicitly. They are boundary conditions that are likely to be used exactly in subsequent steps, so we really want to make sure they actually satisfy constraints like collision and aren't just near another point that satisfies constraints.

[mkoval]

This is an important point. I agree your conclusion:

we should always check the start and goal endpoints' constraints explicitly.

Does this affect any planners besides SnapPlanner? Could you put together a pull request that makes the necessary changes?

[psigen]

At the moment, SnapPlanner is the only planner I've directly observed with this behavior, but we should probably also be on the lookout for similar things in VectorFieldPlanner.

[DavidB-CMU]

I disagree with that conclusion.
If you have robot-to-object collision checking problems at the end point, then you can just as easily have an object that may collide at some part way along the trajectory.
This suggests our calculations for the collision checking resolution are wrong.

You should upload some specific test cases for this.
I have seen your problem myself, but it was actually related to problems with incorrect collision geometry, incorrect loading of collision geometry, or incorrect cloning of an environment.

[psigen]

@DavidB-CMU unless we are secretly doing inflated collision volumes or swept bounding-box checks, yes, you can have an object that may collide at some part of the trajectory. There are many constraints that we're simply incapable of completely verifying or satisfying in a computationally efficient way. This goes especially for most trajectory-wide constraints.

That doesn't change the fact that we should verify our constraints at least exactly at the endpoints. They are the most important points at which to not have a constraint violation since we need to interface with other actions that only know about the trajectory satisfying these constraints. We can (and must) approximate the constraint satisfaction in other places.

[DavidB-CMU]

Isn't HERB's collision geometry an inflated volume? or is it just simplified geometry?

What kind of constraints?
We should separate the topics of trajectory-wide constraints and collision-checking.

Regarding collision-checking:
My understanding is that @mkoval and @cdellin chose our collision-checking resolution to do at least enough collision checks. But I may be mistaken.
If the collision-checking resolution is very small, then it's easy to see that you are sweeping out a swept volume using the robot's motion around the volume of the objects in the world.
And obviously there will be overlap in the collision geometry of subsequent checks.

Here's one way to show that simply adding the end point is not sufficient:

• First add your test case for grasping using the TSR near the table surface.
• Then create a case where you slide the gripper along the table surface for 20cm before grasping the object.
  This motion will have similar "collision checking problems" as the problem you're describing with the end-point.

[mkoval]

What kind of constraints? We should separate the topics of trajectory-wide constraints and collision-checking.

I disagree. "Non-collision" is just another trajectory-wise constraint from the planner's perspective. In fact, we represent it as such in Aikido.

The key feature is that both types of constraints can only be evaluated at a configuration. This would not be true if we had a trajectory-wide constraint that we can analytically evaluate for a line segment (e.g. checking whether the endpoints lie inside a convex set of valid configurations).

If the collision-checking resolution is very small, then it's easy to see that you are sweeping out a swept volume using the robot's motion around the volume of the objects in the world.

This is only true in the limit. For any non-trivial robot it is pretty easy to find a counter-example for an arbitrarily small collision detection resolution. Imagine adding a point obstacle at the midpoint between those two checks. The only way to guarantee that a trajectory is non-colliding is to either pad the environment or impose additional constraints on the obstacles in it (e.g. a minimum size).

Here's one way to show that simply adding the end point is not sufficient:

I agree that there is nothing special about the last point from the perspective of guaranteeing that the trajectory is collision free. However, I also think that explicitly checking the end-point of the trajectory serves an important purpose because we call a sequence of motion planners. See my reply in #303 for an explanation of what I mean.

I agree that this should not be implemented in the Van der Corput sequence - or, at least, not enabled by default. Each planner should be responsible for checking its own endpoint.