dair_pll.drake_system

Interface with Drake MultibodyPlant simulation.

Interfacing with Drake is done by massaging a drake system into the System interface defined in system.py with a new inheriting type, DrakeSystem.

A large portion of the internal implementation of DrakeSystem is contained in MultibodyPlantDiagram in drake_utils.py.

class dair_pll.drake_system.DrakeSystem(urdfs, dt, visualization_file=None)[source]

Bases: System

System wrapper of a Drake simulation environment for a MultibodyPlant.

Drake simulation is constructed as a Simulator of a Diagram in a member MultibodyPlantDiagram variable. States are converted between StateSpace and Drake formats via DrakeStateConverter.

Inits DrakeSystem with provided model URDFs.

Parameters:

  • urdfs (Dict[str, str]) – Names and corresponding URDFs to add as models to plant.

  • dt (float) – Time step of plant in seconds.

  • visualization_file (Optional[str]) – Optional output GIF filename for trajectory visualization.

space: dair_pll.state_space.ProductSpace
plant_diagram: dair_pll.drake_utils.MultibodyPlantDiagram
dt: float
urdfs: typing.Dict[str, str]
preprocess_initial_condition(x_0, carry_0)[source]

Preprocesses initial condition state sequence into single state initial condition for integration.

Parameters:

  • x_0 (Tensor) – (T_0, space.n_x) initial state sequence.

  • carry_0 (Tensor) – (1, ?) initial hidden state.

Return type:

Tuple[Tensor, Tensor]

Returns:

(1, space.n_x) processed initial state. (1, ?) processed initial hidden state.

get_quantized_start_time(start_time)[source]

Get phase-aligned start time for Drake Simulator.

As Drake models time stepping as events in a continuous time domain, some special care must be taken to ensure each call to DrakeSystem.step() triggers one update. This is done by offsetting the simulation duration to advance to N * dt + dt/4 to prevent accidentally taking 2 or 0 steps with a call to step().

Parameters:

start_time (float) – Time step beginning time.

Return type:

float

Returns:

Time step quantized starting time.

sim_step(x, carry)[source]

Simulate forward in time one step.

Parameters:

  • x (Tensor) – (n_x,) current state.

  • carry (Tensor) – (?,) current hidden state.

Return type:

Tuple[Tensor, Tensor]

Returns:

(n_x,) next state. (?,) next hidden state.