IrisInConfigurationSpace with free bodies

Question

I want to run IrisInConfigurationSpace with a plant that contains free bodies. However, I'm getting the following error because these bodies don't have position limits.

Failure at geometry/optimization/iris.cc:447 in IrisInConfigurationSpace(): condition 'plant.GetPositionLowerLimits().array().isFinite().all()' failed.

How can I resolve this issue? Is there a better way than creating an additional plant that contains the bodies' collision geometries as welded bodies?

For additional context, my plant contains an iiwa and free body manipulands. I want to create IRIS regions so that I can create iiwa plans with GCS. These plans should avoid the free bodies.

Answer 1

There are perhaps three very different ways of solving this problem:

1. Calling IrisInConfigurationSpace with the floating bodies would be growing regions in the configuration space of your IIWA + all of the bodies. That can get big fast... so big that it is likely not what you want. But if you did compute those regions, then you could move the bodies and still have valid regions. You're right that we ask for all configuration variables to be bounded in IRIS, so we would need something in the API to make it easy to add bounds on the floating base quantities OR handle the floating base coordinates explicitly. (https://arxiv.org/abs/2305.06341 is working towards a possible recipe)
2. You can certainly weld the obstacles and compute the regions. That would compute regions only in the configuration of the robot -- which seems right. But then you've locked in the locations of the objects, and in most cases we want the objects to move.
3. In https://arxiv.org/abs/2303.14737, Mark described in part the recipe that we use most often -- we compute the iris regions without the movable objects in the scene (e.g. just the iiwa and the shelves), and then before each plan we trim the iris regions with the one or two new obstacles in their current configuration. That is likely the best solution for now.