CODEWITHSUNDEEP
drake
jwelde
jwelde

Reputation: 25

Affine relationship between input and output ports in Drake

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!

Upvotes: 0

Views: 98

Answers (2)

Russ Tedrake
Russ Tedrake

Reputation: 5533

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.

Upvotes: 1

Hongkai Dai
Hongkai Dai

Reputation: 2766

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

Upvotes: 1

Related Questions