How can I get a feedthrough from the plant state port to my leaf system port for a simple shape in Drake?

Question

I have a simple system with a block, without actuation. I would like to create a controller that: gets the state of the block, uses this information in a custom Leaf System, which then publishes a force on the block to move it. However, the custom Leaf System does not seem to have a direct feedthrough with the state output port of the block system. The calculation of the force does not get triggered.
How can I get the controller to run in a loop with this simple system? Here is a an example system implement in pydrake.

from pydrake.all import *
from pydrake.common.cpp_param import List
from pydrake.common.value import Value
from pydrake.geometry import (
    DrakeVisualizer,
    DrakeVisualizerParams,
    Role,
)
import numpy as np


class SpatialForceSystem(LeafSystem):
    def __init__(self, body):
        LeafSystem.__init__(self)
        print("I'm a spatial force leaf system.")
        self.body = body
        forces_cls = Value[List[ExternallyAppliedSpatialForce_[float]]]
        self.DeclareAbstractOutputPort("spatial_forces",
                                       lambda: forces_cls(),
                                       self.publish_force)
        self.DeclareVectorInputPort("f_sol", 3)

    def publish_force(self, context, spatial_forces_vector):
        print("Publishing a force.")
        f = self.get_input_port(self.f_port_idx).Eval(context)
        force = ExternallyAppliedSpatialForce_[float]()
        spatial_force = SpatialForce(
            tau=[0, 0, 0],
            f=f)
        force.F_Bq_W = spatial_force
        force.p_BoBq_B = np.zeros(3)
        force.body_index = self.body.index()
        spatial_forces_vector.set_value([force])


class ForceGeneratorSystem(LeafSystem):
    def __init__(self, ns):
        LeafSystem.__init__(self)
        print("I'm a force generator system.")
        self.DeclareVectorInputPort("box_state", ns)
        self.DeclareVectorOutputPort("f_sol", 3, self.calculate_force)

    def calculate_force(self, context, output):
        print("Calculating force.")
        x = self.get_input_port(self.state_port_idx).Eval(context)
        output.set_value([0., 0., 0.])


def add_box(plant):
    box_idx = plant.AddModelInstance("box")
    table_box = plant.AddRigidBody(
        "base_link_box", box_idx, SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
    plant.RegisterCollisionGeometry(
        table_box, RigidTransform(), Box(0.1, 0.1, 0.1), "box",
        CoulombFriction(1., 1.))
    plant.RegisterVisualGeometry(
        table_box, RigidTransform(), Box(0.1, 0.1, 0.1), "box",
        [.7, 0.1, 0.1, 0.8])
    return box_idx


# Initialize the system
dt = 0.05
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, dt)
plant.mutable_gravity_field().set_gravity_vector(np.array((0, 0, 0)))
box_idx = add_box(plant)
plant.Finalize()

box_body = plant.GetBodyByName("base_link_box")
force_sys = builder.AddSystem(SpatialForceSystem(box_body))

ns = plant.num_multibody_states()
gen_sys = builder.AddSystem(ForceGeneratorSystem(ns))

builder.Connect(plant.get_state_output_port(),
                gen_sys.get_input_port(0))

builder.Connect(gen_sys.get_output_port(0),
                force_sys.get_input_port(0))

params = DrakeVisualizerParams(
    role=Role.kProximity, show_hydroelastic=True)
DrakeVisualizer(params=params).AddToBuilder(builder, scene_graph)
ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph)

# Finalize the diagram
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(
    plant, diagram_context)

# Set initial state
q0 = np.array([1., 0., 0., 0., 0.0, 0.0,  0.1])
v0 = np.zeros(plant.num_velocities())
x0 = np.hstack((q0, v0))
plant.SetPositionsAndVelocities(plant_context, box_idx, x0)

diagram.ForcedPublish(diagram_context)

simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(True)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_at_initialization(True)
simulator.Initialize()
simulator.AdvanceTo(5)

When I run the above code, I only get: "I'm a spatial force leaf system." and "I'm a force generator system." but I am expecting to see: "Calculating force." and "Publishing a force." I also tried to use the DependencyTicket on all state outputs, but that did not help.

Answer 1

The reason that your force calculation is not triggered is because you put the SpatialForce in an output port of your SpatialForceSystem system and nothing is requesting the value of that output port. The computation is only carried out when the output port is pulled.

For example, if you want the spatial force to act on the box in your MultibodyPlant, you can do builder.Connect(force_sys.get_output_port(0), plant.get_applied_spatial_force_input_port()). Then (after fixing a couple of other things in the script you provided), you should be able to see "Publishing a force." every time step.