HPolyhedron.UniformSample returns the same point repeatedly when polyhedron has measure 0

shaoyuancc commented 4 months ago

What happened?

When you have a HPolyhedron with measure 0, e.g. because of equality constraints, the Uniform sampling doesn't generate uniform samples.

from pydrake.all import (HPolyhedron, RandomGenerator)
import numpy as np

# Create a polyhedron which is the line between (1,1) and (3,1)
A_p = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]])
b_p = np.array([3, -1, 1, -1])
P = HPolyhedron(A_p, b_p)

n_samples = 10
samples = []
generator = RandomGenerator()
initial_guess = P.MaybeGetFeasiblePoint()
try:
    samples.append(P.UniformSample(generator, initial_guess))
    for i in range(n_samples - 1):
        samples.append(
            P.UniformSample(generator, previous_sample=samples[-1])
        )
except:
    print("Warning: failed to sample convex set")

print(samples)

returns

[array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.]), array([1., 1.])]

@cohnt @rhjiang

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

No response

cohnt commented 4 months ago

Should be easy enough to fix -- we could just project the direction onto the basis of the affine hull of the HPolyhedron. But I'd rather not get the AffineSubspace code mixed up in this class if the set is full-dimensional.

sherm1 commented 4 months ago

Assigned to Russ for disposition.

cohnt commented 1 month ago

I would like to fix this bug. Mathematically, I know how to fix it, but I have software design questions.

From a user-oriented perspective, we should better clarify the behavior of this method when the set is measure zero (or unbounded -- I assume it throws?) We can definitely warn the user that the set may be measure zero if the max and min values of the line turn out to be the same, up to tolerance.
From a performance perspective, all of the computations related to detecting measure zero and computing the projection map should only be done once. But they also shouldn't be done for every HPolyhedron ever created, since it's a nontrivial computation, and the user may know that it's unnecessary.

Based on this, I suggest we update the documentation of the current UniformSample method, and add in the warning about the polytope not being full dimensional, but otherwise leave the method unchanged.

One option is to add an optional argument to the UniformSample method, denoting a linear subspace along which to draw directions. The user could then compute the affine hull, and provide the basis of that affine hull as an argument.

Another option is to add a function somewhere (in HPolyhedron? in AffineSubspace? somewhere else?) that projects an HPolyhedron onto its affine hull, yielding a new HPolyhedron object that is full-dimensional. This function would return the new HPolyhedron and matrices describing the affine transformation between the coordinate systems. The user could then draw samples from the new HPolyhedron object, and simply map them back with a matrix multiplication and vector addition.

RussTedrake commented 1 month ago

All of those proposals seem reasonable to me. Thank you.

It might be important to understand how @shaoyuancc ran into this in the first place? Was it from inside an IRIS loop? In that case only throwing an error and providing a new entry point might not fix the use case?

cohnt commented 1 month ago

I'm pretty sure Shao ran into this due to sets that were explicitly constructed with equality constraints. Such sets arise in GCS due to the affine equality constraints joining trajectory segments in adjacent regions. I think it's extremely difficult to get this sort of region from IRIS (the seed point would have to be perfectly between two obstacles, and the stepback distance would have to perfectly align with that distance). So I'm not too worried about that.

Russ, feel free to assign to me, and I'll try to get to this next week. My preference would be adding an extra (optional) argument to the UniformSample function, and leave it to the user to provide a basis of the affine hull. In documenting that optional argument, we can direct the user to the code in Drake that will do so automatically.

RobotLocomotion / drake