CODEWITHSUNDEEP
drake
Rufus
Rufus

Reputation: 5566

GetInfeasibleConstraints runtime error: Output must be of scalar type float

I'm trying to debug my MP with the following

solver = IpoptSolver()
result = solver.Solve(prog)
result.GetInfeasibleConstraints(prog)

However, I get the following error:

PyFunctionConstraint: Output must be of scalar type float. Got AutoDiffXd instead.

I have autodiff constraints added similar to the way in the compass gait example.

e.g.

plant_autodiff = plant.ToAutoDiffXd()
def eq7h(q_v_r):
    q, v, r = np.split(q_v_r, [
        plant.num_positions(),
        plant.num_positions() + plant.num_velocities()])
    context = plant_autodiff.CreateDefaultContext()
    plant_autodiff.SetPositions(context, q)
    plant_autodiff.SetVelocities(context, v)
    return plant_autodiff.CalcCenterOfMassPosition(context) - r
prog.AddConstraint(eq7h, lb=[0]*3, ub=[0]*3, vars=np.concatenate([q[k], v[k], r[k]]))

How do I use GetInfeasibleConstraints correctly?

Upvotes: 1

Views: 115

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

GetInfeasibleConstraints requires the constraint to be evaluated with a vector of doubles. So I would re-write your constraint as

def eq7h(q_v_r):
    q, v, r = np.split(q_v_r, [
        plant.num_positions(),
        plant.num_positions() + plant.num_velocities()])
    # Select plant based on the data type of q_v_r. If the data type is autodiff (np.object in this case), then use plant_autodiff, otherwise use plant
    plant_eval = plant_autodiff if q_v_r.dtype == np.object else plant
    context = plant_eval.CreateDefaultContext()
    plant_eval.SetPositions(context, q)
    plant_eval.SetVelocities(context, v)
    return plant_eval.CalcCenterOfMassPosition(context) - r

Upvotes: 1

Related Questions