def simulate(builder, plant, position, regulator, x0, duration):
""""Actually run simulation and return data."""
# Set up plant and position
builder.Connect(plant.get_output_port(0), position.get_input_port(0))
controller = builder.AddSystem(regulator)
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up logging
plant_logger = LogOutput(plant.get_output_port(0), builder)
position_logger = LogOutput(position.get_output_port(0), builder)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
context.SetContinuousState(x0)
# Create the simulator and simulate
simulator = Simulator(diagram, context)
simulator.Initialize()
print("Simulating...")
simulator.AdvanceTo(duration)
print("Simulation complete!")
# Return data
assert len(plant_logger.sample_times()) == len(
position_logger.sample_times())
for t1, t2 in zip(plant_logger.sample_times(),
position_logger.sample_times()):
assert t1 == t2
t = plant_logger.sample_times()
r_data = plant_logger.data()[0, :]
r_dot_data = plant_logger.data()[1, :]
theta_dot_data = plant_logger.data()[2, :]
phi_data = plant_logger.data()[3, :]
phi_dot_data = plant_logger.data()[4, :]
theta_data = position_logger.data()[0, :]
return t, r_data, r_dot_data, theta_dot_data, phi_data, phi_dot_data, theta_data
def test_lcm_driven_loop(self):
"""Duplicates the test logic in `lcm_driven_loop_test.cc`."""
lcm_url = "udpm://239.255.76.67:7669"
t_start = 3.
t_end = 10.
def publish_loop():
# Publishes a set of messages for the driven loop. This should be
# run from a separate process.
# N.B. Because of this, care should be taken not to share C++
# objects between process boundaries.
t = t_start
while t <= t_end:
message = header_t()
message.utime = int(1e6 * t)
lcm.Publish("TEST_LOOP", message.encode())
time.sleep(0.1)
t += 1
class DummySys(LeafSystem):
# Converts message to time in seconds.
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("header_t",
AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def _calc_output(self, context, output):
message = self.EvalAbstractInput(context, 0).get_value()
y = output.get_mutable_value()
y[:] = message.utime / 1e6
# Construct diagram for LcmDrivenLoop.
lcm = DrakeLcm(lcm_url)
utime = mut.PyUtimeMessageToSeconds(header_t)
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
builder = DiagramBuilder()
builder.AddSystem(sub)
dummy = builder.AddSystem(DummySys())
builder.Connect(sub.get_output_port(0), dummy.get_input_port(0))
logger = LogOutput(dummy.get_output_port(0), builder)
logger.set_forced_publish_only()
diagram = builder.Build()
dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime)
dut.set_publish_on_every_received_message(True)
# N.B. Use `multiprocessing` instead of `threading` so that we may
# avoid issues with GIL deadlocks.
publish_proc = Process(target=publish_loop)
publish_proc.start()
# Initialize to first message.
first_msg = dut.WaitForMessage()
dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg))
# Run to desired amount. (Anything more will cause interpreter to
# "freeze".)
dut.RunToSecondsAssumingInitialized(t_end)
publish_proc.join()
# Check expected values.
log_t_expected = np.array([4, 5, 6, 7, 8, 9])
log_t = logger.sample_times()
self.assertTrue(np.allclose(log_t_expected, log_t))
log_y = logger.data()
self.assertTrue(np.allclose(log_t_expected, log_y))
def test_lcm_driven_loop(self):
"""Duplicates the test logic in `lcm_driven_loop_test.cc`."""
lcm_url = "udpm://239.255.76.67:7669"
t_start = 3.
t_end = 10.
def publish_loop():
# Publishes a set of messages for the driven loop. This should be
# run from a separate process.
# N.B. Because of this, care should be taken not to share C++
# objects between process boundaries.
t = t_start
while t <= t_end:
message = header_t()
message.utime = int(1e6 * t)
lcm.Publish("TEST_LOOP", message.encode())
time.sleep(0.1)
t += 1
class DummySys(LeafSystem):
# Converts message to time in seconds.
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort(
"header_t", AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(
BasicVector(1), self._calc_output)
def _calc_output(self, context, output):
message = self.EvalAbstractInput(context, 0).get_value()
y = output.get_mutable_value()
y[:] = message.utime / 1e6
# Construct diagram for LcmDrivenLoop.
lcm = DrakeLcm(lcm_url)
utime = mut.PyUtimeMessageToSeconds(header_t)
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
builder = DiagramBuilder()
builder.AddSystem(sub)
dummy = builder.AddSystem(DummySys())
builder.Connect(sub.get_output_port(0), dummy.get_input_port(0))
logger = LogOutput(dummy.get_output_port(0), builder)
logger.set_forced_publish_only()
diagram = builder.Build()
dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime)
dut.set_publish_on_every_received_message(True)
# N.B. Use `multiprocessing` instead of `threading` so that we may
# avoid issues with GIL deadlocks.
publish_proc = Process(target=publish_loop)
publish_proc.start()
# Initialize to first message.
first_msg = dut.WaitForMessage()
dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg))
# Run to desired amount. (Anything more will cause interpreter to
# "freeze".)
dut.RunToSecondsAssumingInitialized(t_end)
publish_proc.join()
# Check expected values.
log_t_expected = np.array([4, 5, 6, 7, 8, 9])
log_t = logger.sample_times()
self.assertTrue(np.allclose(log_t_expected, log_t))
log_y = logger.data()
self.assertTrue(np.allclose(log_t_expected, log_y))
log = LogOutput(plant.get_output_port(0), builder)
command = builder.AddSystem(ConstantVectorSource([touchdown_angle]))
builder.Connect(command.get_output_port(0), plant.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
context.SetAccuracy(1e-10)
context.get_mutable_continuous_state_vector().SetFromVector(s[:])
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.6)
print("apex: " + str(plant.last_apex))
t = log.sample_times()
x = log.data()
fig, ax = plt.subplots(9)
ax[0].plot(x[0, :], x[1, :])
ax[0].set_xlabel('x')
ax[0].set_ylabel('z')
ax[0].axis('equal')
for i in range(8):
ax[i+1].plot(t, x[i, :])
ax[i+1].set_xlabel('t')
ax[i+1].set_ylabel(s.get_fields()[i])
plt.show()
def __init__(self):
#import pdb; pdb.set_trace()
# Create a block diagram containing our system.
builder = DiagramBuilder()
# add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2)
plant_x = builder.AddSystem(DoubleIntegrator())
plant_y = builder.AddSystem(DoubleIntegrator())
plant_x.set_name("double_integrator_x")
plant_y.set_name("double_integrator_y")
# add the controller, lead compensator for now just to stablize it
controller_x = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_y = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_x.set_name("controller_x")
controller_y.set_name("controller_y")
# the perception's "Beam" model with the four sources of noise
x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
x_meas_fb.set_name("x_fb")
y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
y_meas_fb.set_name("y_fb")
y_perception = builder.AddSystem(Adder(num_inputs=2, size=1))
y_perception.set_name("range_measurment_y")
neg_y_meas = builder.AddSystem(Gain(k=-1., size=1))
neg_y_meas.set_name("neg_y_meas")
wall_position = builder.AddSystem(ConstantVectorSource([Y_wall]))
p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\
sampling_interval_sec=0.01))
p_hit.set_name("GaussionNoise(0,1)")
p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\
sampling_interval_sec=0.01))
p_short.set_name("ExponentialNoise(1)")
#p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\
# sampling_interval_sec=0.01))
p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\
sampling_interval_sec=0.01))
p_rand.set_name("UniformNoise(0,1)")
#import pdb; pdb.set_trace()
p_hit_Dx = builder.AddSystem(Demultiplexer(size=2))
p_hit_Dx.set_name('Dmux1')
p_short_Dx = builder.AddSystem(Demultiplexer(size=2))
p_short_Dx.set_name('Dmux2')
p_rand_Dx = builder.AddSystem(Demultiplexer(size=2))
p_rand_Dx.set_name('Dmux3')
normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_x.set_name("Sigma_x")
normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_y.set_name("Sigma_y")
expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_x.set_name("Exp_x")
expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_y.set_name("Exp_y")
randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_x.set_name("Rand_x")
randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_y.set_name("Rand_y")
#maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_x.set_name("Max_x")
#maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_y.set_name("Max_y")
# the summation to get the error (closing the loop)
summ_x = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_y = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_x.set_name("summation_x")
summ_y.set_name("summation_y")
neg_x = builder.AddSystem(Gain(k=-1., size=1))
neg_y = builder.AddSystem(Gain(k=-1., size=1))
neg_uy = builder.AddSystem(Gain(k=-1., size=1))
neg_x.set_name("neg_x")
neg_y.set_name("neg_y")
neg_uy.set_name("neg_uy")
# wire up all the blocks (summation to the controller to the plant ...)
builder.Connect(summ_x.get_output_port(0),
controller_x.get_input_port(0)) # e_x
builder.Connect(summ_y.get_output_port(0),
controller_y.get_input_port(0)) # e_y
builder.Connect(controller_x.get_output_port(0),
plant_x.get_input_port(0)) # u_x
builder.Connect(
controller_y.get_output_port(0),
neg_uy.get_input_port(0)) # u_y (to deal with directions)
builder.Connect(neg_uy.get_output_port(0),
plant_y.get_input_port(0)) # u_y
builder.Connect(
plant_x.get_output_port(0),
x_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(wall_position.get_output_port(0),
y_perception.get_input_port(0)) # perception
builder.Connect(plant_y.get_output_port(0),
neg_y_meas.get_input_port(0)) # perception
builder.Connect(neg_y_meas.get_output_port(0),
y_perception.get_input_port(1)) # perception, z meas
builder.Connect(
y_perception.get_output_port(0),
y_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(p_hit.get_output_port(0),
p_hit_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_hit_Dx.get_output_port(0),
normgain_x.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_x.get_output_port(0),
x_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_hit_Dx.get_output_port(1),
normgain_y.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_y.get_output_port(0),
y_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_short.get_output_port(0),
p_short_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_short_Dx.get_output_port(0),
expgain_x.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_x.get_output_port(0),
x_meas_fb.get_input_port(2)) # Exp dist
builder.Connect(p_short_Dx.get_output_port(1),
expgain_y.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_y.get_output_port(0),
y_meas_fb.get_input_port(2)) # Exp dist
#builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist
#builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand.get_output_port(0),
p_rand_Dx.get_input_port(0)) # normalize Uniform dist
builder.Connect(p_rand_Dx.get_output_port(0),
randgain_x.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_x.get_output_port(0),
x_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand_Dx.get_output_port(1),
randgain_y.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_y.get_output_port(0),
y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(x_meas_fb.get_output_port(0),
neg_x.get_input_port(0)) # -x
builder.Connect(y_meas_fb.get_output_port(0),
neg_y.get_input_port(0)) # -y
builder.Connect(neg_x.get_output_port(0),
summ_x.get_input_port(1)) # r-x
builder.Connect(neg_y.get_output_port(0),
summ_y.get_input_port(1)) # r-y
# Make the desired_state input of the controller, an input to the diagram.
builder.ExportInput(summ_x.get_input_port(0))
builder.ExportInput(summ_y.get_input_port(0))
# get telemetry
logger_x = LogOutput(plant_x.get_output_port(0), builder)
logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder)
logger_y = LogOutput(plant_y.get_output_port(0), builder)
logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder)
logger_x.set_name("logger_x_state")
logger_y.set_name("logger_y_state")
logger_noise_x.set_name("x_state_meas")
logger_noise_y.set_name("y_meas")
# compile the system
diagram = builder.Build()
diagram.set_name("diagram")
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# First we extract the subsystem context for the plant
plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context)
plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context)
# Then we can set the pendulum state, which is (x, y).
z0 = X_0
plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0)
z0 = Y_0
plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0)
# The diagram has a single input port (port index 0), which is the desired_state.
context.FixInputPort(
0,
[X_d]) # here we assume no perception, closing feedback on state X
context.FixInputPort(
1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0
# run the simulation
simulator.AdvanceTo(10)
t = logger_x.sample_times()
#import pdb; pdb.set_trace()
# Plot the results.
plt.figure()
plot_system_graphviz(diagram, max_depth=2)
plt.figure()
plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas')
plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))')
plt.plot(t, logger_x.data().transpose(), label='x_state')
plt.plot(t, logger_y.data().transpose(), label='y_state')
plt.xlabel('t')
plt.ylabel('x(t),y(t)')
plt.legend()
plt.show(block=True)
# y = x
def CopyStateOut(self, context, output):
x = context.get_continuous_state_vector().CopyToVector()
output.SetFromVector(x)
continuous_system = SimpleContinuousTimeSystem()
# Simulation
# Create a simple block diagram containig our system
builder = DiagramBuilder()
# system = builder.AddSystem(SimpleContinuousTimeSystem())
system = builder.AddSystem(continuous_vector_system)
logger = LogOutput(system.get_output_port(0), builder)
diagram = builder.Build()
# Set the initial conditions, x(0)
context = diagram.CreateDefaultContext()
context.SetContinuousState([0.9])
# Create the simulator, and simulate for 10 seconds
simulator = Simulator(diagram, context)
simulator.AdvanceTo(10)
# Plot the results
plt.figure()
plt.plot(logger.sample_times(), logger.data().transpose())
plt.xlabel('t')
plt.ylabel('y(t)')
plt.show()
log = LogOutput(plant.get_output_port(0), builder)
command = builder.AddSystem(ConstantVectorSource([touchdown_angle]))
builder.Connect(command.get_output_port(0), plant.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
context.SetAccuracy(1e-10)
context.get_mutable_continuous_state_vector().SetFromVector(s[:])
simulator.set_target_realtime_rate(1.0)
simulator.StepTo(0.6)
print("apex: " + str(plant.last_apex))
t = log.sample_times()
x = log.data()
fig, ax = plt.subplots(9)
ax[0].plot(x[0, :], x[1, :])
ax[0].set_xlabel('x')
ax[0].set_ylabel('z')
ax[0].axis('equal')
for i in range(8):
ax[i+1].plot(t, x[i, :])
ax[i+1].set_xlabel('t')
ax[i+1].set_ylabel(s.get_fields()[i])
plt.show()
