我们能否以符号形式缓存方程式并在轨迹优化期间插入数字？

Question

这是否可以预先计算雅可比矩阵，符号化质量矩阵并仅在优化过程中对其进行评估？例如，我们可以将 M = plant.CalcMassMatrixViaInverseDynamics(context) 缓存为符号矩阵。添加约束时，只需编写一个函数将数字状态插入符号 M.

我认为通过这种方式，我们不需要在约束评估期间创建上下文。这会加快速度吗？或者德雷克实际上在做类似的事情？

Answer 1

我不建议在您的非线性约束中评估符号表达式。直接使用 autodiff 标量计算 M 要快得多。我建议您创建一个约束，例如

def my_constraint(plant, context, q, v):
  # Returns M[:, 0] as the constraint value.
  plant.SetPositionsAndVelocities(context, q, v)
  M = plant.CalcMassMatrixViaInverseDynamics(context)
  if q.dtype == np.object:
    # If the input type is autodiff scalars, then return M[:, 0] as a numpy array of autodiff scalars.
    return M[:, 0]
  elif q.dtype == float:
    # If the input type is float, then return M[:, 0] as a numpy array of floats.
    return pydrake.autodiffutils.autoDiffToValueMatrix(M[:,0])

prog.AddConstraint(lambda x: my_constraint(plant, context, x[:nq], x[nq:nq+nv]), lower_bound, upper_bound, vars)

其中 plant 是一个带有 autodiffscalar 的 MultibodyPlant 实例化（您可以调用 ToAutoDiff() 将带有 float 或符号表达式的 MultibodyPlant 实例化转换为带有 autodiff 标量的实例化。

您可以查看 https://github.com/RobotLocomotion/drake/blob/3cae4801eac3f9cc3d948c193022ebb8bfba5124/bindings/pydrake/solvers/test/mathematicalprogram_test.py#L570-L596 中的一个简单示例。此示例说明如何定义您自己的约束评估函数，并在 AddConstraint.

中使用它

你也可以参考这个stack overlow answer

你提到

we don't need to create context during constraint evaluation

我认为您不需要在约束评估期间创建上下文。在上面的示例中，context 仅在调用 AddConstraint 之前创建一次。而在my_constraint里面，context更新为当前的决策变量值q和v。它再也不会被创建。

我们能否以符号形式缓存方程式并在轨迹优化期间插入数字？

can we cache equations in symbolic form and plug in numbers during trajectory optimization?

drake