def build_block_diagram(boat, controller):
builder = DiagramBuilder()
boat = builder.AddSystem(boat)
boat.set_name('boat')
# logger to track trajectories
logger = LogOutput(boat.get_output_port(0), builder)
logger.set_name('logger')
# expose boats input as an input port
builder.ExportInput(boat.get_input_port(0))
# build diagram
diagram = builder.Build()
diagram.set_name('diagram')
return diagram
def simulate_drake_system(plant):
# Create a simple block diagram containing our system.
builder = DiagramBuilder()
system = builder.AddSystem(plant)
logger = LogOutput(system.get_output_port(0), builder)
diagram = builder.Build()
# Set the initial conditions, x(0).
context = diagram.CreateDefaultContext()
context.SetContinuousState([0, 0, 20, 10, 0, 0])
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram, context)
simulator.AdvanceTo(30)
# Plotting
x_sol = logger.data().T
plot_trj_3_wind(x_sol[:, 0:3], np.array([0, 0, 0]))
plt.show()
return 0
def simulate(q0, qdot0, sim_time, controller):
# initialize block diagram
builder = DiagramBuilder()
# add system and controller
double_integrator = builder.AddSystem(get_double_integrator())
controller = builder.AddSystem(controller)
# wirw system and controller
builder.Connect(double_integrator.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), double_integrator.get_input_port(0))
# measure double-integrator state and input
state_logger = LogOutput(double_integrator.get_output_port(0), builder)
input_logger = LogOutput(controller.get_output_port(0), builder)
# finalize block diagram
diagram = builder.Build()
# instantiate simulator
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False) # makes sim faster
# set initial conditions
context = simulator.get_mutable_context()
context.SetContinuousState([q0, qdot0])
# run simulation
simulator.AdvanceTo(sim_time)
# unpack sim results
q_sim, qdot_sim = state_logger.data()
u_sim = input_logger.data().flatten()
t_sim = state_logger.sample_times()
return q_sim, qdot_sim, u_sim, t_sim
Hence, by running the following cell multiple times, you would actually try to wire the blocks in the diagram more than once.
This is not acceptable, and Drake will raise the error `RuntimeError: Input port is already wired.`
If you wish to modify the next cell and rerun the program to see the effects of your modification, you must rerun the cell where the `builder` is initialized first (i.e. the cell with the line `builder = DiagramBuilder()`).
"""
# instantiate controllers
inner_controller = builder.AddSystem(InnerController(vibrating_pendulum))
outer_controller = builder.AddSystem(OuterController(vibrating_pendulum, pendulum_torque))
# instantiate visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1.5, 2.5], show=False)
)
# logger that records the state trajectory during simulation
logger = LogOutput(vibrating_pendulum.get_state_output_port(), builder)
# mux block to squeeze the (base + pendulum) state and
# the outer control signal in a single cable
mux = builder.AddSystem(Multiplexer([4, 1]))
# selector that extracts the pendulum state from the state of the base and the pendulum
selector = builder.AddSystem(MatrixGain(np.array([
[0, 1, 0, 0], # selects the angle of the pendulum
[0, 0, 0, 1] # selects the angular velocity of the pendulum
])))
# (base + pendulum) state, outer control -> mux
builder.Connect(vibrating_pendulum.get_state_output_port(), mux.get_input_port(0))
builder.Connect(outer_controller.get_output_port(0), mux.get_input_port(1))
def lyapunov_simulation(
x0,
xf,
uref=0,
create_dynamics=create_symbolic_dynamics,
to_point=False,
a=0.1,
u_max=0.5,
eps=2.):
'''
Simulates and plots our Lyapunov Controller in different scenarios.
Parameters:
x0: initial state (x, y, yaw)
x1: final state (x, y, yaw)
uref: reference input
create_dynamics: symbolic dynamics creation function
a, eps: lyapunov controller parameters
u_max: control limit (u_max = 1/r)
'''
params = np.array([a, u_max, eps])
uavPlant = create_dynamics()
# construction site for our closed-loop system
builder = DiagramBuilder()
# add the robot to the diagram
# the method .AddSystem() simply returns a pointer to the system
# we passed as input, so it's ok to give it the same name
uav = builder.AddSystem(uavPlant)
# add the controller
if not to_point:
controller = builder.AddSystem(LyapunovController(lyapunov_controller, params, xf))
else:
controller = builder.AddSystem(LyapunovController(lyapunov_controller_to_point, uref, xf))
# wire the controller with the system
builder.Connect(uavPlant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), uavPlant.get_input_port(0))
# add a logger to the diagram
# this will store the state trajectory
logger = LogOutput(uavPlant.get_output_port(0), builder)
control_logger = LogOutput(controller.get_output_port(0), builder)
# complete the construction of the diagram
diagram = builder.Build()
# reset logger to delete old trajectories
# this is needed if you want to simulate the system multiple times
logger.reset()
# set up a simulation environment
simulator = Simulator(diagram)
# simulator.set_target_realtime_rate(1)
# set the initial cartesian state to a random initial position
# try initial_state = np.random.randn(3) for a random initial state
context = simulator.get_mutable_context()
context.SetContinuousState(x0)
# simulate from zero to sim_time
# the trajectory will be stored in the logger
if not to_point:
sim_time = 400
else:
sim_time = 0.1
simulator.AdvanceTo(sim_time)
# print('done!')
# draw_simulation(logger.data())
return (simulator, logger.data(), control_logger.data())
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.ticker import StrMethodFormatter
from pydrake.all import (DiagramBuilder, LogOutput, Simulator,
SymbolicVectorSystem, Variable)
builder = DiagramBuilder()
x = Variable("x")
plant = builder.AddSystem(
SymbolicVectorSystem(state=[x],
dynamics=[4 * x * (1 - x)],
output=[x],
time_period=1))
log = LogOutput(plant.get_output_port(0), builder)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
random_state = np.random.RandomState() # see drake #12632.
fig, ax = plt.subplots(2, 1)
for i in range(2):
context.SetTime(0)
context.SetDiscreteState([random_state.uniform()])
log.reset()
simulator.Initialize()
simulator.AdvanceTo(200)
estimated_state_mux.get_input_port(3))
builder.Connect(estimated_state_demux.get_output_port(8),
estimated_state_mux.get_input_port(4))
builder.Connect(estimated_state_demux.get_output_port(10),
estimated_state_mux.get_input_port(5))
# Now we can feed the repackaged states to the controller
builder.Connect(estimated_state_mux.get_output_port(0),
controller.get_input_port_estimated_state())
# Close the loop by connecting the output of the controller to the plant
builder.Connect(controller.get_output_port_control(),
pancake_flipper.get_actuation_input_port())
# We also want to measure the control input for funsies
u_logger = LogOutput(controller.get_output_port_control(), builder)
# Also measure the states
q_logger = LogOutput(pancake_flipper.get_state_output_port(), builder)
# Add a visualizer so we can see what's going on
max_flipper_x = np.max(q_opt[:, 0])
max_pancake_x = np.max(q_opt[:, 1])
min_flipper_x = np.min(q_opt[:, 0])
min_pancake_x = np.min(q_opt[:, 1])
max_x = max(max_flipper_x, max_pancake_x)
min_x = min(min_flipper_x, min_pancake_x)
max_flipper_y = np.max(q_opt[:, 2])
max_pancake_y = np.max(q_opt[:, 3])
min_flipper_y = np.min(q_opt[:, 2])
min_pancake_y = np.min(q_opt[:, 3])
x0[6] = .0433371122 # z on ground = .0433371122
x0[7] = 0 # right wheel
x0[8] = 0 # left wheel
x0[12] = 0. #x_dot
#null_controller = builder.AddSystem(u0)
#builder.Connect(null_controller.get_output_port(0), plant.get_input_port(0))
#Controller as in lqr.py for 3d quadrotor
controller = builder.AddSystem(Diff_Drive_Controller(plant))
builder.Connect(plant.get_continuous_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
#Data Logger
logger = LogOutput(plant.get_continuous_state_output_port(), builder)
#Run Simulation
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.)
plant_context = diagram.GetMutableSubsystemContext(
plant, simulator.get_mutable_context())
print plant_context.get_mutable_discrete_state_vector()
plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)
simulator.Initialize()
simulator.StepTo(duration)
#print('sample_times',logger.sample_times()) #nx1 array
# add the robot to the diagram # the method .AddSystem() simply returns a pointer to the system # we passed as input, so it's ok to give it the same name robot = builder.AddSystem(robot) # add the controller controller = builder.AddSystem(ParkingController(lyapunov_controller)) # wire the controller with the system builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) # add a logger to the diagram # this will store the state trajectory logger = LogOutput(robot.get_output_port(0), builder) # complete the construction of the diagram diagram = builder.Build() """## Simulate the Closed-Loop System It's time to simulate the closed-loop system: just pass the diagram we constructed to the simulator and press play! """ # reset logger to delete old trajectories # this is needed if you want to simulate the system multiple times logger.reset() # set up a simulation environment simulator = Simulator(diagram)