Drake输入输出端口间的仿射关系

Question

假设我有一个带有输入端口和输出端口的DrakeDiagram。我碰巧知道，保持图表的状态不变，这些输入和输出端口具有仿射关系，即y = A(x) u + b(x)，其中x为Diagram的完整状态。然而，该图不是明确的AffineSystem，因为它对状态的依赖是非线性的。

对于图的给定状态，生成相应的AffineSystem或提取矩阵a和b的最佳方法是什么?目前，我使用

# 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()

的1e20容限基本上只是迫使德雷克线性化系统，即使它不是在一个平衡。但我想知道是否有更有原则的方法来做到这一点。

我想这样做的原因类似于这个用户关于欠驱动系统的输入分配的问题。对于由MultibodyPlant,Propeller和Multiplexer组成的图，它将输入端口复用到Propeller命令和JointActuator命令，我想计算输入和广义加速度之间的仿射关系，因此我可以在QP中使用此数据进行输入分配。

提前感谢您的帮助!

Answer 1

这可能不是一个很好的方法，但是您可以考虑通过以下过程

获得a (x)和b(x)

1. 首先修复u为0，然后xdot只是b(x)，就像你已经做过的。
2. 现在修复u为[1,0,0，…]， 0]，则xdot = b(x) + A(x).col(0)。从这个结果中减去步骤1中得到的b(x)，剩下的是A(x).col(0)。
3. 重复步骤2，将u设置为单位向量e ` `，使e ` ` (j) = δ(i, j)。然后得到A(x)的第i列。

Answer 2

如果您愿意，还有另一种可能性。线性化使用AutoDiffXd来完成这项工作。我们还提供了FirstOrderTaylorApproximation，它可以在没有平衡检查的情况下有效地线性化。你可以自己做autodiff调用，只对u求导(例如标称u = 0)，它会给你你所需要的。

Hongkai的方法的优点是不需要和AutoDiffXd版本的系统。