CODEWITHSUNDEEP
drake
Luyao
Luyao

Reputation: 83

How to deal with the algebraic loop in Drake?

I wrote the code for a linear system and a simple state feedback controller in Drake, but when connecting the two systems, the following error message appeared, RuntimeError: Reported algebraic loop detected in DiagramBuilder:. Then, I replaced my linear system with LinearSystem in Drake, and the program worked this time. What is wrong with my way of modeling linear systems, and Why does LinearSystem work? This is my code snippet:

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

Upvotes: 3

Views: 153

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

Great question. I can definitely see how that is confusing. Long story short, I believe you just need to pass a DependencyTicket to your call to DeclareVectorOutputPort that does not include the input port (only the state).

I am pretty sure that the reason that we don't have that in the C++ code is that the C++ code also support symbolic scalar-types, so the Diagram is able to infer this non-dependency by evaluating the symbolic version of the system. But your python class only supports doubles, so the diagram logic is limited in it's ability to introspect, and must assume the worst. In this case you can simply declare the (non-)dependency explicitly.

IMHO, it would be a nice improvement to the TimeVaryingAffineSystem base class to check if D=0, and declare the non-dependency here, too. That would avoid the non-trivial expense of converting to symbolic. We'd welcome a PR! :-)

Upvotes: 3

Related Questions