求Drake中Simulink Sources/From Workspace的等效自定义数组输入模块

Got it, I’ve dealt with this exact scenario when working with Drake—replicating Simulink’s From Workspace functionality to feed a custom numpy array sequence into your simulation is totally doable with a small custom LeafSystem. The built-in RandomSource and ConstantVectorSource don’t fit here because they can’t follow a predefined sequence, so a custom discrete system is the way to go.

Core Approach #

We’ll create a discrete LeafSystem that holds your [n,K] numpy array, tracks the current simulation step, and outputs the corresponding column at each discrete time interval. This system will sync with your simulation’s discrete update steps (or a fixed dt you define) to match the behavior of Simulink’s block.

Complete Code Implementation #

First, here’s the custom source system:

import numpy as np
from pydrake.systems.framework import LeafSystem, BasicVector
from pydrake.systems.analysis import Simulator

class FromWorkspaceSource(LeafSystem):
    def __init__(self, data: np.ndarray, dt: float):
        super().__init__()
        # Store input data (shape: [n, K], n=input dimensions, K=number of steps)
        self._data = data
        self._dt = dt
        self._num_steps = data.shape[1]

        # Declare output port matching your data's row count
        self.DeclareVectorOutputPort(
            "output",
            BasicVector(data.shape[0]),
            self.CalcOutput
        )

        # Discrete state to track current step (clean way to persist state across updates)
        self.DeclareDiscreteState(1)
        # Initialize discrete state to step 0
        self.SetDefaultDiscreteState(np.array([0.]))

        # Declare periodic update to increment step at each dt interval
        self.DeclarePeriodicDiscreteUpdateEvent(
            period_sec=dt,
            offset_sec=0.0,
            update=self.UpdateCurrentStep
        )

    def CalcOutput(self, context, output):
        # Get current step from discrete state
        current_step = int(context.get_discrete_state_vector().GetAtIndex(0))
        # Clamp to last step if we exceed available data to avoid index errors
        active_step = min(current_step, self._num_steps - 1)
        # Output the corresponding column from your data array
        output.SetFromVector(self._data[:, active_step])

    def UpdateCurrentStep(self, context, discrete_state):
        # Increment step only if we haven't reached the end of the data
        current_step = int(context.get_discrete_state_vector().GetAtIndex(0))
        if current_step < self._num_steps - 1:
            discrete_state.get_mutable_vector().SetAtIndex(0, current_step + 1)

How to Use This System #

Integrate this into your existing simulation setup like so:

# 1. Define your custom input data (replace with your actual array)
n_inputs = 2  # Number of dimensions in each input vector
num_steps = 50  # Number of time steps in your sequence
dt = 0.2  # Discrete time step matching your controller/plant's update rate
custom_data = np.random.randn(n_inputs, num_steps)  # Your [n,K] numpy array

# 2. Instantiate the workspace source
workspace_source = FromWorkspaceSource(custom_data, dt)

# 3. Connect to your plant/controller (use your actual port names)
# Assuming you have a DiagramBuilder set up
# builder = DiagramBuilder()
# plant = builder.AddSystem(your_plant_system)
# controller = builder.AddSystem(your_controller_system)
# Connect the source's output to your controller's input port
# builder.Connect(workspace_source.get_output_port(), controller.get_input_port("your_input_port_name"))

# 4. Build and run the simulation
# diagram = builder.Build()
# simulator = Simulator(diagram)
# simulator.AdvanceTo(num_steps * dt)  # Run for the full length of your data

Key Notes #

• Sync with Simulation Step: The dt you pass to the system should match the discrete update rate of your controller or plant to ensure the input aligns correctly with each simulation step.
• Data Handling: If your simulation runs longer than the length of your data array, the system will automatically hold the last column of your data (no index errors!).
• Flexibility: You can modify this system to handle time-based indexing instead of step-based if needed (e.g., if your simulation uses variable time steps), but step-based works perfectly for discrete-time setups like you described.

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