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()