如何在多体工厂中使用 MathematicalProgram 中的决策变量 (TypeError)

Question

我正在尝试为我的数学程序添加动力学约束。但是，我得到如下粘贴的 TypeError，表明 MBP 对连续变量作为输入不满意。

我是否需要以某种方式将符号变量转换为数值，或者是否应该使用其他类型的变量？

相关代码如下：

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))

错误信息如下：

--------------------------------------------------------------------------- 
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)

Answer 1

您需要 plant 用符号变量实例化。当你打电话

model = Parser(plant).AddModelFromFile(file_name)

它构造一个 plant 用 python 浮点数实例化。你可以打电话给

plant_symbolic = plant.ToSymbolic()

ToSymbolic() 函数创建一个新的 plant_symbolic 接受符号变量 x 和 u.

Answer 2

只是因为您在此处和 class 讨论板上发帖时得到的答案不同，让我再补充一个想法。您可以通过几种不同的方式添加约束：

def myconstraint(vars):
    return constraint_values

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

或

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

第一种方式，如果你使用 def myconstraint 意味着你应该提供一个接受 AutoDiffXd 的函数（并且需要 plant.ToAutoDiffXd）。这是一种更通用的方法，因为 AutoDiffXd 支持更多我们可以用 symbolic::Expression 支持的计算。例如，AutoDiffXd 处理了 MultibodyPlant 中的许多联系案例，但符号却没有。

如果您可以获得计算的符号版本（使用 plant.ToSymbolic），那么您可以将约束添加为 symbolic::Expression。对于非常大的计算，这可能会很慢——而且对于 MBP 的某些部分，它将不受支持。但是这个工作流程的一个令人惊奇的事情是，当它工作时，MathematicalProgram 足够聪明，可以在 symbolic::Expression 是线性的（在变量中）等时添加线性约束，等等。所以它实际上可以计算知道你有一个二次程序，然后调用更专业的求解器。

但是根据您的工作流程，我怀疑您使用 AutoDiffXd 版本会走得更快，就像我们在罗盘步态问题集上所做的那样。

如何在多体工厂中使用 MathematicalProgram 中的决策变量 (TypeError)

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

drake