Drake中如何实现Plant到Leaf System的端口馈通以运行控制循环？

我拥有一个无驱动的方块系统，希望构建一款控制器：获取方块的状态，在自定义Leaf System中利用该信息计算并发布作用力以驱动方块。但自定义Leaf System与Plant的状态输出端口似乎无直接馈通，力计算逻辑未被触发。运行以下代码时，仅输出「I'm a spatial force leaf system.」和「I'm a force generator system.」，但预期能看到「Calculating force.」和「Publishing a force.」的输出。我也曾尝试使用DependencyTicket处理所有状态输出，但并未解决问题。

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)

关键问题与修复步骤 #

1. 补全系统闭环连接
   Drake的系统调度基于需求驱动，只有当输出被下游系统需要时才会触发计算。原代码中SpatialForceSystem的空间力输出没有连接到MultibodyPlant的外部力输入端口，导致整个控制器链路被判定为无作用，因此不会执行。需要添加连接：

   builder.Connect(force_sys.get_output_port(0), plant.get_applied_spatial_force_input_port())
   

2. 修复未定义的端口索引
   两个自定义LeafSystem中使用的self.f_port_idx和self.state_port_idx未在初始化时赋值，运行时会引发错误。需要在声明端口时保存索引：

   • 在SpatialForceSystem的__init__中修改输入端口声明：

     self.f_port_idx = self.DeclareVectorInputPort("f_sol", 3).get_index()
     

   • 在ForceGeneratorSystem的__init__中修改输入端口声明：

     self.state_port_idx = self.DeclareVectorInputPort("box_state", ns).get_index()
     

修复后的完整代码 #

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.f_port_idx = self.DeclareVectorInputPort("f_sol", 3).get_index()

    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.state_port_idx = self.DeclareVectorInputPort("box_state", ns).get_index()
        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))

# 新增：将空间力输出连接到Plant的外部力输入端口，形成闭环
builder.Connect(force_sys.get_output_port(0), plant.get_applied_spatial_force_input_port())

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)

运行修复后的代码，即可看到「Calculating force.」和「Publishing a force.」的周期性输出，控制器与方块系统形成完整的运行循环。

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