如何聚合多系统的List[ExternallyAppliedSpatialForce]至MultibodyPlant

问题本质 #

MultibodyPlant.get_applied_spatial_force_input_port() 是单一输入端口，不允许直接连接多个输出端口，多次连接必然触发 RuntimeError: Input port is already wired.。你需要先把多个控制器输出的 List[ExternallyAppliedSpatialForce] 合并成一个列表，再传入该端口。

实现方案 #

1. 编写空间力聚合器 #

自定义LeafSystem来合并多组空间力：

from pydrake.systems.framework import LeafSystem
from pydrake.multibody.tree import ExternallyAppliedSpatialForce
from typing import List

class SpatialForceAggregator(LeafSystem):
    def __init__(self, input_count: int):
        super().__init__()
        # 声明输入端口，数量与控制器数量一致
        self._input_ports = [
            self.DeclareAbstractInputPort(
                f"controller_force_{i}",
                value_type=List[ExternallyAppliedSpatialForce]()
            )
            for i in range(input_count)
        ]
        # 声明聚合后的输出端口
        self.DeclareAbstractOutputPort(
            "combined_forces",
            alloc=lambda: List[ExternallyAppliedSpatialForce](),
            calc=self._combine_forces
        )

    def _combine_forces(self, context, output):
        combined = []
        for port in self._input_ports:
            combined.extend(port.Eval(context))
        output.set_value(combined)

2. 端口连接流程 #

• 实例化聚合器：aggregator = SpatialForceAggregator(num_inputs=2)（对应主、次级两个控制器）
• 将主控制器、次级控制器的空间力输出端口分别连接到聚合器的两个输入端口
• 将聚合器的 combined_forces 输出端口连接到 MultibodyPlant 的 applied_spatial_force_input_port

3. 适配浮动体控制器 #

确保每个浮动体控制器的输出是 List[ExternallyAppliedSpatialForce] 类型（单个力也要包装成列表），保证聚合器能正常合并。

额外提示 #

• 所有空间力必须对应 MultibodyPlant 中已定义的刚体，否则仿真会报错
• 若要模拟控制器延迟，可在控制器与聚合器之间插入 Delay 系统，满足双边控制的延迟分析需求

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