How to use decision variable from MathematicalProgram in a multibody plant (TypeError)

Question

I'm trying to add dynamics constraints to my mathematical program. However, I get TypeError as pasted below indicating MBP is unhappy with a continuous variable being an input.

Do I need to convert the symbolic variable to numerical values somehow or is there another type of variable I should use?

Here's relevant code:

def add_decision_variables(prog, n_steps):
    # dimensions of x and u
    dim_x = 6
    dim_u = 2

    x = prog.NewContinuousVariables(rows=n_steps+1, cols=dim_x) # states, x[t] = [q1, q2, q3, q1_dot, q2_dot, q3_dot]
    u = prog.NewContinuousVariables(rows=n_steps+1, cols=dim_u) # input u[t] = [tau1, tau2]
    # ... other variables...

    return x, u



def add_dynamics_contstraint(prog, n_steps, decision_variables, time_step=0.0001): 
    # build plant in diagram
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
    file_name = FindResource("models/robot.urdf")
    model = Parser(plant).AddModelFromFile(file_name)
    plant.Finalize()
    diagram = builder.Build()

    x = decision_variables[0] # states, x[t] = [q1, q2, q3, q1_dot, q2_dot, q3_dot]
    u = decision_variables[1] # [q1, q2] because q3 is not controllable

    for t in range(n_steps-1):     
        prog.AddLinearConstraint(eq(x[t,0:3] - x[t+1,0:3] - time_step*x[t+1,3::], 0))

Here's the error message:

--------------------------------------------------------------------------- 
TypeError 
Traceback (most recent call last) <ipython-input-22-f2bd5b25729d> in <module>

     10 set_initial_and_goal_position(prog, decision_variables)
     11 add_contact_constraint(prog, n_steps, decision_variables)
---> 12 add_dynamics_contstraint(prog, n_steps, decision_variables)
     13 
     14 

<ipython-input-21-ff72bdc2eedf> in add_dynamics_contstraint(prog, n_steps, decision_variables, time_step)
     30 
     31         # Set plant dynamics to current state
---> 32         plant.SetPositionsAndVelocities(plant_context, x[t+1, :])
     33 
     34         # Use the plant for dynamics parameters

TypeError: SetPositionsAndVelocities(): incompatible function arguments. The following argument types are supported:
    1. (self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], q_v: numpy.ndarray[float64[m, 1]]) -> None
    2. (self: pydrake.multibody.plant.MultibodyPlant_[float], context: pydrake.systems.framework.Context_[float], model_instance: pydrake.multibody.tree.ModelInstanceIndex, q_v: numpy.ndarray[float64[m, 1]]) -> None

Invoked with: <pydrake.multibody.plant.MultibodyPlant_[float] object at 0x133f885b0>, <pydrake.systems.framework.LeafContext_[float] object at 0x133fa5a30>, array([Variable('x(1,0)', Continuous), Variable('x(1,1)', Continuous),
       Variable('x(1,2)', Continuous), Variable('x(1,3)', Continuous),
       Variable('x(1,4)', Continuous), Variable('x(1,5)', Continuous)],
      dtype=object)

score 2 · Accepted Answer · answered May 10 '20 at 00:29

Just because you got different answers when you posted both here and on the class discussion board, let me add one more thought. You can add constraints in a few different ways:

def myconstraint(vars):
    return constraint_values

prog.AddConstraint(myconstraint, lb, ub, vars)

or

prog.AddConstraint(expression, lb, ub)
prog.AddConstraint(lb <= expression) 
...

The first way, if you use def myconstraint means you should provide a function that accepts AutoDiffXd (and will need plant.ToAutoDiffXd). This is a more general approach, as AutoDiffXd supports more computation that we can possibly support with symbolic::Expression. For example, AutoDiffXd works through many cases of contact in MultibodyPlant, but symbolic does not.

If you can obtain a symbolic version of your computation (using plant.ToSymbolic) then you can add the constraint as a symbolic::Expression. For very big calculations, this could be slow -- and for some parts of MBP, it will be unsupported. But one of the amazing things about this workflow, when it works, is that MathematicalProgram is smart enough to add linear constraints when the symbolic::Expression is linear (in the variables), etc. So it can actually figure out that you have a quadratic program, and call the more specialized solver.

But based on your workflow, I suspect you will get farther faster with the AutoDiffXd version, like we did on the compass gait problem set.

score 1 · Answer 2 · answered May 09 '20 at 23:52

You will need a plant instantiated with symbolic variables. When you call

model = Parser(plant).AddModelFromFile(file_name)

it constructs a plant instantiated with python float. You could call

plant_symbolic = plant.ToSymbolic()

ToSymbolic() function creates a new plant_symbolic that accepts symbolic variables x and u.

How to use decision variable from MathematicalProgram in a multibody plant (TypeError)

2 Answers2

Linked