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

user_6680310 0
Viewed 71

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?

drake
1 Answers

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.

user_19738610
Related