DirectCollocation does not work with distance constraint due to a context issue.

[hongkai-dai]

What happened?

After calling DirectCollocation(plant, plant_context, ...) to construct DirectCollocation object, it internally clones this context https://github.com/RobotLocomotion/drake/blob/17caa3837fafa9385994b2d31d8167ea1139a523/planning/trajectory_optimization/direct_collocation.cc#L179-L182

On the other hand, if we use a DistanceConstraint in DirectCollocation, DinstanceConstraint expects the context is constructed in this way

auto diagram_context = diagram.CreateDefaultContext();
auto plant_context = plant.GetMyMutableContextFromRoot(diagram_context);

so that the plant_context is aware of the SceneGraph.

If we pass this plant_context into DirectCollocation, it will try to clone the context, causing an error

line 135, in solve_dircol
    dircol = DirectCollocation(
RuntimeError: Context::Clone(): Cannot clone a non-root Context; this Context was created by 'plant'.

cc @JoseBarreiros-TRI

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

No response

[hongkai-dai]

Never mind, I think this is the solution

diagram_context = diagram.CreateDefaultContext()
auto dir_col = DirectCollocation(diagram, diagram_context, ...)

# Allocating contexts for each waypoints
diagram_contexts_waypoints = [None] * dircol.N()
plant_contexts_waypoints = [None] * dircol.N()
for i in range(dir_col.N()):
    # Add kinematic constraint within this for loop
    diagram_contexts_waypoints[i] = diagram.CreateDefaultContext()
    plant_contexts_waypoints[i] = plant.GetMyMutableContextFromRoot(diagram_contexts_waypoints[i])
    dir_col.prog().AddConstraint(DistanceConstraint(plant, geometry_pair, plant_contexts_waypoints[i], distance_upper, distance_lower), dircol.state(i)[:plant.num_positions()])
    # Similarly also add other kinematic constraints, like PositionConstraint, OrientationConstraint, etc within this for loop;

The idea is to create the context to be used for DistanceConstraint (and other kinematic constraint that would also require a context) separately, not using the context passed into the constructor of DirectCollocation.