Open sadraddini opened 5 months ago
I prefer giving the user the option to specify which constraints to consider first (the user additional constraints or the collision geometry), rather than hard-coding to an order. I think it is always possible to find the case that one order works better than the other.
I prefer giving the user the option to specify which constraints to consider first (the user additional constraints or the collision geometry), rather than hard-coding to an order. I think it is always possible to find the case that one order works better than the other.
I agree. The example to your point is when collision-free region is much smaller than the constraint-satisfying set.
However, I would not be sad with switching the order in the code if it is the "easier" option. Typically collision constraints are much more expensive to handle.
I'll be working on this. Have some other ideas.
This is probably going to be resolved by IRIS-NP2 (#21822).
Is your feature request related to a problem? Please describe. I'm calling
IrisInConfigurationSpace
with nonlinear constraints on the config. I notice that in the code,When the nonlinear constraints are too tight (the set that satisfies it is much smaller than the collison-free region), most of the hyperplanes in the first step, related to collision geos, are wasted. They are never active, and they eat most of the computation time.
Here is an example. Consider the following URDF:
Let's plot the 2D config space. Reds are collisions, greens are collision-free.
Now say I want a collision-free IRIS region where one of the configs is tightly constrained:
1.37 < q[0] < 1.57
. For suchq[0]
, the set ofq[1]
that is collision-free is the whole range ofq[1]
.Now, if I describe the constraint as a nonlinear one with
and get it to the
prog_with_additional_constraints
, I will give very slow computation of the IRIS region.Describe the solution you'd like Switch the order: First compute the hyperplanes from the constraints (they are cheaper), and then add the hyperplanes from geometry. Or at least give the user the option for that.
In my example, most of the computation for the collision geos for the blue region is wasteful as soon as black line constraint comes into play.
Describe alternatives you've considered How I unlocked myself: I manually linearized the constraints. Linear constraints are processed before the collision geos and thus this problem goes away.
Additional context I noticed this problem when computing IRIS regions for dual robot arms, when one of the arms is constrained to be in particular pose, far from the other am. The expected result is that nearly the whole config space of the other robot must be collision-free. But running IRIS builder, I get very small IRIS regions, and this issue is the source of the problems.