Drake中固定状态下提取Diagram输入输出仿射关系A、b矩阵的方法问询

解答 #

你原来的实现逻辑是正确的，但通过设置极大容差绕开平衡点检查的方式确实不够规范，Drake提供了专门的API来满足你的需求，具体实现可以参考如下方案：

通用规范实现方法 #

Drake内置的LinearizeInputOutput函数专门用于计算任意工作点下的输入输出雅可比矩阵，本身不会对状态是否为平衡点做校验，完全不需要调整容差参数，是你需求场景下的最优选择。
完整规范实现代码如下：

import numpy as np
from pydrake.systems.analysis import LinearizeInputOutput

# 1. 将Context状态设置为目标值x
context.get_mutable_continuous_state_vector().SetFromVector(x)
input_port = diagram.get_input_port(input_index)
output_port = diagram.get_output_port(output_index)
nu = input_port.size()

# 2. 计算仿射项b（输入为0时的输出值）
input_port.FixValue(context, np.zeros(nu))
b = output_port.Eval(context)

# 3. 计算输入到输出的线性系数矩阵A
io_linear_system = LinearizeInputOutput(
    system=diagram,
    context=context,
    input_port_index=input_index,
    output_port_index=output_index
)
A = io_linear_system.D()

针对你的场景的更高性能优化方案 #

由于你的需求是计算输入和广义加速度的仿射关系，不需要对整个Diagram做线性化，直接调用底层API可以获得更高的计算效率，更适合QP求解时的高频调用场景：

• 从MultibodyPlant中直接获取质量矩阵、偏置项、作动矩阵
• 单独计算Propeller的推力到广义力的映射矩阵
• 自行拼接得到输入到广义加速度的A、b矩阵，省去整个Diagram的线性化开销

生成AffineSystem实例 #

如果需要生成固定状态下的AffineSystem对象，拿到A和b后直接构造即可：

from pydrake.systems.primitives import AffineSystem

nx = context.num_continuous_states()
ny = output_port.size()
affine_sys = AffineSystem(
    A=np.zeros((nx, nx)),
    B=np.zeros((nx, nu)),
    C=np.zeros((ny, nx)),
    D=A,
    x0=np.zeros(nx),
    u0=np.zeros(nu),
    y0=b
)

内容的提问来源于stack exchange，提问作者jwelde