What does 'RuntimeError: PyFunctionConstraint: Output must be of scalar type AutoDiffXd, but unable to infer scalar type.' mean?

Question

When attempting to solve my mathematical program, drake produces the RuntimeError: PyFunctionConstraint: Output must be of scalar type AutoDiffXd, but unable to infer scalar type.

The constraint responsible for the error is as follows:

def non_penetration_constraint(q):
    plant_ad.SetPositions(context_ad, q)
    Distances = np.zeros(6, dtype=q.dtype)
    i = 0
    for name in Claw_name:
        g_ids = body_geometries[name]
        assert(len(g_ids) == 1)
        Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(
            Worldbody_id, g_ids[0]).distance
        i += 1

    return Distances

Called with:

for n in range(N-1):
    # Non penetration constraint
    prog.AddConstraint(non_penetration_constraint,
                       lb = [0] * nf,
                       ub = [0] * nf,
                       vars = (q[:, n]))

Has anybody had a similar issue before?

Answer 1

I suspect that your query_object (and perhaps other values) are not from the autodiff version of your plant/scene_graph.

Constraints implemented like this can potentially be called with q[0] either a type float or type AutoDiffXd. See the "writing custom evaluator" section of this Drake tutorial. In your case, the return value from your constraint is coming out of the query_object, which is not impacted directly by the plant_ad.SetPositions call (which seems suspicious). You'll want to make sure the query_object is generated from the autodiff scene_graph, and presumably after you set the positions to your constraint value.