Iris Region Calculation can be faster with tight nonlinear constraints

[sadraddini]

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,

• first, the hyperplanes related to collision are generated
• second, the hyperplanes from the nonlinear constraints are added

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:

<robot name="robot">
  <link name="arm_1">
    <collision>
      <geometry><box size="1 0.3 1"/></geometry>
    </collision>
  </link>
  <link name="arm_2">
    <collision>
      <geometry><box size="1 0.3 1"/></geometry>
    </collision>
  </link>
  <joint name="joint1" type="revolute">
    <origin rpy="0 0 0" xyz="-0.35 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-1.57" upper="1.57"/>
    <parent link="world"/>
    <child link="arm_1"/>
  </joint>
  <joint name="joint2" type="revolute">
    <origin rpy="0 0 0" xyz="0.35 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-1.57" upper="1.57"/>
    <parent link="world"/>
    <child link="arm_2"/>
  </joint>
</robot>"""

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 such q[0], the set of q[1] that is collision-free is the whole range of q[1].

Now, if I describe the constraint as a nonlinear one with

ik = InverseKinematics(plant)
prog = ik.prog()
q_var = ik.q()
threshold = 0.98
prog.AddConstraint(np.sin(q_var[0]) >= threshold)

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.

[hongkai-dai]

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.

[sadraddini]

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.

[sadraddini]

I'll be working on this. Have some other ideas.

[RussTedrake]

This is probably going to be resolved by IRIS-NP2 (#21822).

[cohnt]

Have you tried out these examples with IrisZo? Presumably, if IrisZo does well with these scenarios, then IRIS-NP2 will as well.

A separate question is whether we should let the user determine if additional constraints are checked before or after collisions (in the IrisZo/IrisNP2 sampling paradigm). I would think usually, we want to check the constraints first, but if the constraints are computationally intensive, it could take longer than the collision checking.