drake

import numpy as np from pydrake.systems.framework import DiagramBuilder, Simulator from pydrake.examples.acrobot import AcrobotPlant from pydrake.systems.primitives import LogVectorOutput # Create a DiagramBuilder builder = DiagramBuilder() # Add the Acrobot plant (a classic underactuated system) acrobot = builder.AddSystem(AcrobotPlant()) # Add a logger to record the state of the Acrobot logger = LogVectorOutput(acrobot.get_output_port(0), builder) # Build the diagram, connecting the components diagram = builder.Build() # Create a simulator for the diagram simulator = Simulator(diagram) context = simulator.get_mutable_context() # Set initial state: (theta1, theta2, theta1_dot, theta2_dot) # e.g., inverted upright with a small perturbation context.SetContinuousState([np.pi/2, 0.1, 0.0, 0.0]) # Simulate for 2 seconds simulator.AdvanceTo(2.0) # Retrieve and print some logged data (optional verification) log = logger.FindLog(context) time = log.sample_times() positions = log.data()[:2, :] velocities = log.data()[2:, :] print(f"Simulation finished. Logged {len(time)} time steps.") print(f"Initial positions: {positions[:, 0]}, Final positions: {positions[:, -1]}")