def RunSimulation(quadrotor_plant, control_law, x0=np.random.random((8, 1)), duration=30, control_period=0.0333, print_period=1.0, simulation_period=0.0333): quadrotor_controller = QuadrotorController(control_law, control_period=control_period, print_period=print_period) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( QuadrotorPendulum(mb=quadrotor_plant.mb, lb=quadrotor_plant.lb, m1=quadrotor_plant.m1, l1=quadrotor_plant.l1, g=quadrotor_plant.g, input_max=quadrotor_plant.input_max)) controller = builder.AddSystem(quadrotor_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant input_log = builder.AddSystem(SignalLogger(2)) input_log._DeclarePeriodicPublish(control_period, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(8)) state_log._DeclarePeriodicPublish(control_period, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() simulator.set_publish_every_time_step(True) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(control_period) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
def RunSimulation(pendulum_plant, control_law, x0=np.random.random((4, 1)), duration=30): pendulum_controller = PendulumController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( InertialWheelPendulum(m1=pendulum_plant.m1, l1=pendulum_plant.l1, m2=pendulum_plant.m2, l2=pendulum_plant.l2, r=pendulum_plant.r, g=pendulum_plant.g, input_max=pendulum_plant.input_max)) controller = builder.AddSystem(pendulum_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant input_log = builder.AddSystem(SignalLogger(1)) input_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(4)) state_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() simulator.set_publish_every_time_step(False) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(0.005) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
def RunSimulation(robobee_plantBS, control_law, x0=np.random.random((15, 1)), duration=30): robobee_controller = RobobeeController(control_law) # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() # The last pendulum plant we made is now owned by a deleted # system, so easiest path is for us to make a new one. plant = builder.AddSystem( RobobeePlantBS(m=robobee_plantBS.m, Ixx=robobee_plantBS.Ixx, Iyy=robobee_plantBS.Iyy, Izz=robobee_plantBS.Izz, g=robobee_plantBS.g, input_max=robobee_plantBS.input_max)) Rigidbody_selector = builder.AddSystem(RigidBodySelection()) print("1. Connecting plant and controller\n") controller = builder.AddSystem(robobee_controller) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to capture the simulation of our plant print("2. Connecting plant to the logger\n") set_time_interval = 0.001 time_interval_multiple = 1000 publish_period = set_time_interval * time_interval_multiple input_log = builder.AddSystem(SignalLogger(4)) # input_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(controller.get_output_port(0), input_log.get_input_port(0)) state_log = builder.AddSystem(SignalLogger(15)) # state_log._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Drake visualization print("3. Connecting plant output to DrakeVisualizer\n") rtree = RigidBodyTree( FindResourceOrThrow("drake/examples/robobee/robobee_arena.urdf"), FloatingBaseType.kQuaternion) #robobee_twobar lcm = DrakeLcm() visualizer = builder.AddSystem( DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True)) builder.Connect(plant.get_output_port(0), Rigidbody_selector.get_input_port(0)) builder.Connect(Rigidbody_selector.get_output_port(0), visualizer.get_input_port(0)) print("4. Building diagram\n") diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. print("5. Create simulation\n") simulator = Simulator(diagram, context) simulator.Initialize() # simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1) simulator.get_integrator().set_fixed_step_mode(True) simulator.get_integrator().set_maximum_step_size(0.05) # Simulate for the requested duration. simulator.StepTo(duration) return input_log, state_log
diagram = builder.Build() # Set the initial conditions for the simulation. context = diagram.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector(x0) # Create the simulator. simulator = Simulator(diagram, context) simulator.Initialize() # simulator.set_publish_every_time_step(Falspe) simulator.set_target_realtime_rate(1) simulator.get_integrator().set_fixed_step_mode(False) simulator.get_integrator().set_maximum_step_size(0.005) # Simulate for the requested duration. simulator.StepTo(duration) # input_log, state_log = \ # RunSimulation(robobee_plant, # test_LQRcontroller, # x0=x0, # duration=duration) num_iteration = np.size(state_log.data(), 1) num_state = np.size(state_log.data(), 0) state_out = state_log.data()