Drake运行时错误：PyFunctionConstraint无法推断AutoDiffXd标量类型求助

求解数学规划问题时，Drake抛出如下RuntimeError：

PyFunctionConstraint: Output must be of scalar type AutoDiffXd, but unable to infer scalar type.

触发该错误的约束函数代码如下：

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

调用该约束的代码为：

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

请问有谁遇到过类似问题吗？

内容的提问来源于stack exchange，提问作者Remi Makinwa