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

Question

Is this possible to precompute the jacobians, mass matrix symbolically and only evaluate them in the optimization process? e.g., we can cache M = plant.CalcMassMatrixViaInverseDynamics(context) as a symbolic matrix. When adding constraints, just write a function to plug the numeric states into the symbolic M.

I think in this way, we don't need to create context during constraint evaluation. Will this speed things up? Or drake is actually doing similar things?

Hongkai Dai · Accepted Answer · 2020-05-11T17:04:15.063

I would not recommend evaluating symbolic expression in your nonlinear constraint. It is much faster to compute M using autodiff scalars directly. I would recommend that you create a constraint like

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)

where plant is a MultibodyPlant instantiate with autodiffscalar (you could call ToAutoDiff() to convert a MultibodyPlant instantiate with float or symbolic expression to one instantiate with autodiff scalar.

You could check a simple example in https://github.com/RobotLocomotion/drake/blob/3cae4801eac3f9cc3d948c193022ebb8bfba5124/bindings/pydrake/solvers/test/mathematicalprogram_test.py#L570-L596. This example shows how to define your own constraint evaluation function, and use it in AddConstraint.

Also you could refer to this stack overlow answer https://stackoverflow.com/a/61705629/1973861

You mentioned that

we don't need to create context during constraint evaluation

I don't think you need to create context during constraint evaluation. In the example above, context is created before calling AddConstraint for only once. And inside my_constraint, context is updated to the current decision variable value q and v. It is never created again.

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

1 Answers1