0

Say I have a Drake Diagram with an input port and and output port. I happen to know that, holding the state of the diagram constant, these input and output ports have an affine relationship, i.e. y = A(x) u + b(x) where x is the full state of the Diagram. However, the Diagram is not explicitly an AffineSystem since its dependence on state is nonlinear.

What is the best way to generate a corresponding AffineSystem, or extract the matrices A and b for a given state of the diagram? Currently, I'm using

# set state
context.get_mutable_continuous_state_vector().SetFromVector(x)

# inputs to zero to get affine term
diagram.get_input_port().FixValue(context, np.zeros(k))
b = diagram.get_output_port().Eval(context)

# get linear term coefficient matrix
linearized = Linearize(
       diagram,
       context,
       input_index,
       output_index,
       equilibrium_check_tolerance=1e20
    )
A = linearized.D()

where the 1e20 tolerance basically just forces Drake to linearize the system even though it's not at an equilibrium. But I'm wondering if there's a more principled way to do this.

The reason I want to do this is similar to the question of this user about input allocation for underactuated systems. For a diagram consisting of a MultibodyPlant, Propeller, and a Multiplexer which multiplexes the input port to Propeller commands and JointActuator commands, I'd like to compute the affine relationship between inputs and generalized accelerations, so I can use this data in a QP for input allocation.

Thanks in advance for your help!

jwelde
  • 25
  • 3

2 Answers2

1

This is probably not a great way to do it, but you could consider to obtain A(x) and b(x) via the following process

  1. First fix u to 0, then xdot is just b(x), as you have already done.
  2. Now fix u to [1, 0, 0, ..., 0], then xdot is b(x) + A(x).col(0). You subtract b(x) obtained from step 1 from this result, and what left is A(x).col(0).
  3. You repeat step 2 by setting u to the unit vector eᵢ, such that eᵢ(j) = δ(i, j). Then you get the i'th column of A(x).
Hongkai Dai
  • 2,546
  • 11
  • 12
  • Thanks for your suggestion! It may not be the most elegant, but it is fairly straightforward and would get the job done. – jwelde Sep 01 '21 at 17:58
1

Here is another possibility, if you like. Linearize is using AutoDiffXd to do this work. We also offer FirstOrderTaylorApproximation, which is effectively Linearize without the equilibrium check. You could make the autodiff calls yourself taking only the derivative with respect to u (eg for nominal u = 0), and it would give you exactly what you need.

Hongkai’s method has the advantage of not requiring and AutoDiffXd version of the system.

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • This makes sense too! And maybe is slightly more compact despite the additional templating requirement. Thanks for your help, I appreciate it! – jwelde Sep 02 '21 at 18:30