drake - 如何处理德雷克中的代数环？

Question

我在 Drake 中编写了一个线性系统和一个简单的状态反馈 Controller 的代码，但是当连接这两个系统时，出现了以下错误信息，RuntimeError:在 DiagramBuilder 中检测到报告的代数环:。然后，我用 Drake 中的 LinearSystem 替换了我的线性系统，这次程序成功了。我对线性系统建模的方式有什么问题，为什么 LinearSystem 可以工作？这是我的代码片段:

class SimpleContinuousTimeSystem(LeafSystem):
    def __init__(self, A, B, C):
        LeafSystem.__init__(self)
        
        num_state = A.shape[0]
        num_input = B.shape[1]
        num_output = C.shape[0]
        
        # Define the state vector
        self.DeclareContinuousState(num_state)  # Three state variables, x1, x2, x3
        # Define the input
        self.DeclareVectorInputPort("u", BasicVector(num_input))
        # Define the output
        self.DeclareVectorOutputPort("y", BasicVector(num_output), self.CalcOutputY) 
        
        self._A = A
        self._B = B
        self._C = C
        
    def DoCalcTimeDerivatives(self, context, derivatives):
        # Get the state vector
        x = context.get_continuous_state_vector().CopyToVector()
        # Get the input
        u = self.get_input_port(0).Eval(context)
    
        xdot = self._A @ x + self._B @ u

        derivatives.get_mutable_vector().SetFromVector(xdot)
        
    # y = Cx
    def CalcOutputY(self, context, output):
        x = context.get_continuous_state_vector().CopyToVector()
        y = self._C @ x

        output.SetFromVector(y)
        
class SimpleStateFeedbackController(LeafSystem):
    def __init__(self, K, num_state, num_input):
        LeafSystem.__init__(self)
        
        self.DeclareVectorInputPort("x", BasicVector(num_state))
        self.DeclareVectorOutputPort("u", BasicVector(num_input), self.DoCalcOutput)
        
        self._K = K
        
    def DoCalcOutput(self, context, output):
        x = self.get_input_port(0).Eval(context)
        u = self._K @ x
        output.SetFromVector(u)

if __name__ == "__main__":
    
    A = np.array([[-1, 0, 0],
                  [1, -1, 0],
                  [0, 1, 0]])
    
    B = np.array([1, 1, 1]).reshape(3,1)
    C = np.identity(3)
    D = np.zeros((3,1))
    
    K = np.zeros((1,3))
    
    # Create a simple block diagram containing our system.
    builder = DiagramBuilder()
    
    system = builder.AddSystem(SimpleContinuousTimeSystem(A, B, C))
    # system = builder.AddSystem(LinearSystem(A, B, C, D, 0))
    controller =  builder.AddSystem(SimpleStateFeedbackController(K, 3, 1))
    
    builder.Connect(system.get_output_port(0), controller.get_input_port(0))
    builder.Connect(controller.get_output_port(0), system.get_input_port(0))
    
    logger = LogOutput(system.get_output_port(0), builder)
    diagram = builder.Build()

    # Set the initial conditions, x1(0), x2(0), x3(0)
    context = diagram.CreateDefaultContext()
    context.SetContinuousState([0.5, 0.5, 0.5])
  
    # Create the simulator
    simulator = Simulator(diagram, context)
    simulator.AdvanceTo(10)

    # Plot the results.
    plt.figure()
    plt.plot(logger.sample_times(), logger.data().transpose())
    plt.xlabel('t')
    plt.ylabel('y(t)')

Answer 1

好问题。我绝对可以看出这是多么令人困惑。长话短说，我相信你只需要通过 DependencyTicket对不包含输入端口(仅状态)的 DeclareVectorOutputPort 的调用。

我很确定我们在 C++ 代码中没有它的原因是 C++ 代码也支持符号标量类型，所以图表能够通过评估符号版本来推断这种非依赖性系统。但是你的 python 类只支持 double ，所以图表逻辑在它的自省(introspection)能力上是有限的，并且必须假设最坏的情况。在这种情况下，您可以简单地显式声明(非)依赖性。

恕我直言，这将是对 TimeVaryingAffineSystem 基类的一个很好的改进，以检查 D=0 并在此也声明非依赖性。这将避免转换为符号的重要费用。我们欢迎 PR! :-)