自定义LeafSystem_对接DirectCollocation输入端口参数报错排查

问题背景 #

在Python 3.11.4 + Drake 1.25.0（WSL2 Ubuntu 23.04）环境下，通过TemplateSystem实现自定义模板化LeafSystem（SlidingBlockSystem_），初始化DirectCollocation时，即使显式指定input_port_index参数，仍触发构造函数参数类型不兼容错误。

报错分析 #

报错信息显示，DirectCollocation的input_port_index参数要求类型为InputPortSelection枚举或InputPortIndex对象，但代码中传递的是get_index()返回的整数，类型不匹配导致报错。此外，代码存在潜在错误：将计算状态导数的DoCalcTimeDerivatives直接作为输出端口的计算函数，会导致输出端口错误返回状态导数而非系统状态。

解决方案 #

1. 修正input_port_index参数类型：
   • 若系统只有一个输入端口，可直接省略input_port_index参数（默认使用InputPortSelection.kUseFirstInputIfItExists）；
   • 若需显式指定，需将整数端口索引包装为InputPortIndex对象。
2. 修复输出端口计算函数：新增专门的输出计算函数，返回系统当前状态而非状态导数。
3. 规范状态导数赋值方式：使用get_mutable_vector()方法修改状态导数，符合Drake API规范。

修改后的完整代码 #

# LeafSystem includes
from pydrake.systems.framework import LeafSystem_, InputPortIndex
from pydrake.systems.scalar_conversion import TemplateSystem
from pydrake.autodiffutils import AutoDiffXd
from pydrake.symbolic import Expression
from pydrake.planning import DirectCollocation
from pydrake.solvers import MathematicalProgram, Solve

import numpy as np
import matplotlib.pyplot as plt


@TemplateSystem.define("SlidingBlockSystem_")
def SlidingBlockSystem_(T):
    class Impl(LeafSystem_[T]):
        def _construct(self, converter=None, mass=2.0, damping=0.5):
            LeafSystem_[T].__init__(self, converter=converter)
            self._mass = mass
            self._damping = damping

            # Ports
            self.DeclareContinuousState(1, 1, 0)
            self._force_port = self.DeclareVectorInputPort(
                "force", size=1)
            # 修复：新增专门的输出计算函数
            self.DeclareVectorOutputPort(
                "state", size=2, calc=self.DoCalcStateOutput)

        def _construct_copy(self, other, converter=None):
            Impl._construct(self, converter=converter,
                            mass=other._mass, damping=other._damping)

        def DoCalcTimeDerivatives(self, context, derivatives):
            x = context.get_continuous_state_vector().GetAtIndex(0)
            xdot = context.get_continuous_state_vector().GetAtIndex(1)
            force = self._force_port.Eval(context).GetAtIndex(0)

            xddot = (force - self._damping*xdot)/self._mass

            # 规范赋值方式
            derivatives.get_mutable_vector().SetAtIndex(0, xdot)
            derivatives.get_mutable_vector().SetAtIndex(1, xddot)

        # 新增：输出端口计算函数，返回当前状态
        def DoCalcStateOutput(self, context, output):
            x = context.get_continuous_state_vector().GetAtIndex(0)
            xdot = context.get_continuous_state_vector().GetAtIndex(1)
            output.SetAtIndex(0, x)
            output.SetAtIndex(1, xdot)

    return Impl


def main():
    N = 25
    tf = 5.0

    sliding_block = SlidingBlockSystem_[AutoDiffXd]()
    context = sliding_block.CreateDefaultContext()

    # 方案1：省略input_port_index（推荐，因仅一个输入端口）
    dircol = DirectCollocation(
        sliding_block, context, num_time_samples=N, 
        minimum_time_step=0.05, maximum_time_step=0.5)
    
    # 方案2：显式指定InputPortIndex对象
    # dircol = DirectCollocation(
    #     sliding_block, context, num_time_samples=N, 
    #     minimum_time_step=0.05, maximum_time_step=0.5,
    #     input_port_index=InputPortIndex(sliding_block.get_input_port().get_index()))
    
    # 后续代码...

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