KinematicTrajectoryOptimization.AddPathPositionConstraint 'lb.size() == num_positions()' error

Question

I'm attempting to add a position constraint to my KinematicTrajectoryOptimization, but am unsure how to resolve this error:

Failure at planning/trajectory_optimization/kinematic_trajectory_optimization.cc:249 in AddPathPositionConstraint(): condition 'lb.size() == num_positions()' failed.

I've tried both APIs of the AddPathPositionConstraint function:

# First method:
start_constraint = PositionConstraint(
    plant,
    plant.world_frame(),
    X_WStart.translation(),  # upper limit
    X_WStart.translation(),  # lower limit
    gripper_frame,
    [0, 0.1, 0],
    plant_context,
)
trajopt.AddPathPositionConstraint(start_constraint, 0)

# Second method:
trajopt.AddPathPositionConstraint(X_WStart.translation(), X_WStart.translation(), 0)

I suspect the error might have to do with the way our MultibodyPlant was constructed; which was by running plant = station.GetSubsystemByName("plant") where station is a HardwareStation, constructed using a scenario with an iiwa arm, WSG gripper, and also an object. The plant, therefore, has 16 positions while the KinematicTrajectoryOptimization has just 7 (for each of iiwa's joints).

Is there some way to create a smaller plant from the original plant that contains just the iiwa? Creating a new plant seems wrong though; the documentation for a PositionConstraint says the plant passed in should be "the MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint."

What is the right solution?

Answer 1

If your KinematicTrajectoryOptimization is operating only over the 7 positions of the iiwa, then you should be using a plant which contains only those 7 in the constraint.

You say "Creating a new plant seems wrong". I think it's perfectly reasonable. This is the model that the robot is using for planning -- it need not be the same model that the physics engine is using for simulation. Very often these are not the same.