secondorder_pid_working.py

from pydrake.symbolic import Variable
from pydrake.systems.analysis import Simulator
from pydrake.systems.controllers import PidController
from pydrake.systems.framework import BasicVector, DiagramBuilder, LeafSystem
from pydrake.systems.primitives import SymbolicVectorSystem

k = 1
s = 1
m = 1

class MassSpringDamper(LeafSystem):
    def __init__(self):
        LeafSystem.__init__(self)
        self.DeclareContinuousState(2)
        self.DeclareVectorOutputPort("y", BasicVector(2), self.CopyStateOut, set([self.all_state_ticket()]))
        self.DeclareVectorInputPort("f", BasicVector(1))

    def DoCalcTimeDerivatives(self, context, derivatives):
        x = context.get_continuous_state_vector().GetAtIndex(0)
        xdot = context.get_continuous_state_vector().GetAtIndex(1)
        f = self.EvalAbstractInput(context, 0).get_value().GetAtIndex(0)
        xddot = (-k*x - s*xdot + f) / m
        derivatives.get_mutable_vector().SetAtIndex(0, xdot)
        derivatives.get_mutable_vector().SetAtIndex(1, xddot)

    def CopyStateOut(self, context, output):
        x = context.get_continuous_state_vector().CopyToVector()
        output.SetFromVector(x)

builder = DiagramBuilder()
plant = builder.AddSystem(MassSpringDamper())
controller = builder.AddSystem(PidController(kp=[1], ki=[1], kd=[1]))

builder.Connect(plant.get_output_port(0), controller.get_input_port_estimated_state())
builder.Connect(controller.get_output_port_control(), plant.get_input_port(0))

builder.ExportInput(controller.get_input_port_desired_state())

diagram = builder.Build()

simulator = Simulator(diagram)
context = simulator.get_mutable_context()
system_context = diagram.GetMutableSubsystemContext(plant, context)
system_context.get_mutable_continuous_state_vector().SetFromVector([0, 0])
context.FixInputPort(0, [10, 0])

simulator.AdvanceTo(30)