def Simulate2dHopper(x0, duration, actuators_off=False, desired_lateral_velocity=0.0, desired_height=3.0, print_period=0.0): # The diagram, plant and contorller diagram = build_block_diagram(actuators_off, desired_lateral_velocity, desired_height, print_period) # Start visualizer recording visualizer = diagram.GetSubsystemByName('visualizer') visualizer.start_recording() simulator = Simulator(diagram) simulator.Initialize() plant = diagram.GetSubsystemByName('plant') plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) potential = plant.CalcPotentialEnergy(plant_context) simulator.AdvanceTo(duration) visualizer.stop_recording() animation = visualizer.get_recording_as_animation() controller = diagram.GetSubsystemByName('controller') state_log = diagram.GetSubsystemByName('state_log') return plant, controller, state_log, animation
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
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(self, real_time_rate=1.0): ''' Here we test using the NNSystem in a Simulator to drive an acrobot. ''' sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile(sdf_file) plant.Finalize(scene_graph) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the plant parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_continuous_state_output_port(), state_log.get_input_port(0)) # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, print_period=print_period)) 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()) # The diagram diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) simulator.StepTo(duration) return plant, controller, state_log
def RunPendulumSimulation(pendulum_plant, pendulum_controller, x0=[0.9, 0.0], duration=10): ''' Accepts a pendulum_plant (which should be a DampedOscillatingPendulumPlant) and simulates it for 'duration' seconds from initial state `x0`. Returns a logger object which can return simulated timesteps ` logger.sample_times()` (N-by-1) and simulated states `logger.data()` (2-by-N). ''' # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() plant = builder.AddSystem(pendulum_plant) 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 # (We tell the logger to expect a 3-variable input, # and hook it up to the pendulum plant's 3-variable output.) logger = builder.AddSystem(SignalLogger(3)) logger.DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) simulator.Initialize() simulator.set_publish_every_time_step(False) # Set the initial conditions for the simulation. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() state.SetFromVector(x0) # Simulate for the requested duration. simulator.AdvanceTo(duration) # Return the logger, which stores the output of the # plant across the time steps of the simulation. return logger
def simulateUntil(t, state_init, u_opt_poly, x_opt_poly, K_poly): ################################################################################## # Setup diagram for simulation diagram = makeDiagram(n_quadrotors, n_balls, use_visualizer=True, trajectory_u=u_opt_poly, trajectory_x=x_opt_poly, trajectory_K=K_poly) simulator = Simulator(diagram) integrator = simulator.get_mutable_integrator() integrator.set_maximum_step_size( 0.01 ) # Reduce the max step size so that we can always detect collisions context = simulator.get_mutable_context() context.SetAccuracy(1e-4) context.SetTime(0.) context.SetContinuousState(state_init) simulator.Initialize() simulator.AdvanceTo(t) return context.get_continuous_state_vector().CopyToVector()
def runManipulationExample(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupDefaultStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.StepTo(args.duration)
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) path = subprocess.check_output([ 'venv/share/drake/common/resource_tool', '-print_resource_path', 'drake/examples/multibody/cart_pole/cart_pole.sdf', ]).strip() Parser(plant).AddModelFromFile(path) plant.Finalize() # Add to visualizer. DrakeVisualizer.AddToBuilder(builder, scene_graph) # Add controller. controller = builder.AddSystem(BalancingLQR(plant)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) diagram = builder.Build() # Set up a simulator to run this diagram. simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() # Simulate. duration = 8.0 for i in range(5): initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4) print(f"Iteration: {i}. Initial: {initial_state}") context.SetContinuousState(initial_state) context.SetTime(0.0) simulator.Initialize() simulator.AdvanceTo(duration)
# Visualize visualizer = builder.AddSystem(pbrv) builder.Connect(rbplant_sys.get_output_port(0), visualizer.get_input_port(0)) # And also log signalLogRate = 60 signalLogger = builder.AddSystem(SignalLogger(nx)) signalLogger.DeclarePeriodicPublish(1. / signalLogRate, 0.0) builder.Connect(rbplant_sys.get_output_port(0), signalLogger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) # TODO(russt): Clean up state vector access below. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() if set_initial_state: initial_state = np.zeros((nx, 1)) initial_state[0] = 1.0 state.SetFromVector(initial_state) simulator.StepTo(args.duration) print(state.CopyToVector())
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
def perform_iou_testing(model_file, test_specific_temp_directory, pose_directory): random_poses = {} # Read camera translation calculated and applied on gazebo # we read the random positions file as it contains everything: with open( test_specific_temp_directory + "/pics/" + pose_directory + "/poses.txt", "r") as datafile: for line in datafile: if line.startswith("Translation:"): line_split = line.split(" ") # we make the value negative since gazebo moved the robot # and in drakewe move the camera trans_x = float(line_split[1]) trans_y = float(line_split[2]) trans_z = float(line_split[3]) elif line.startswith("Scaling:"): line_split = line.split(" ") scaling = float(line_split[1]) else: line_split = line.split(" ") if line_split[1] == "nan": random_poses[line_split[0][:-1]] = 0 else: random_poses[line_split[0][:-1]] = float(line_split[1]) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) model = make_parser(plant).AddModelFromFile(model_file) model_bodies = me.get_bodies(plant, {model}) frame_W = plant.world_frame() frame_B = model_bodies[0].body_frame() if len(plant.GetBodiesWeldedTo(plant.world_body())) < 2: plant.WeldFrames(frame_W, frame_B, X_PC=plant.GetDefaultFreeBodyPose(frame_B.body())) # Creating cameras: renderer_name = "renderer" scene_graph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # N.B. These properties are chosen arbitrarily. depth_camera = DepthRenderCamera( RenderCameraCore( renderer_name, CameraInfo( width=960, height=540, focal_x=831.382036787, focal_y=831.382036787, center_x=480, center_y=270, ), ClippingRange(0.01, 10.0), RigidTransform(), ), DepthRange(0.01, 10.0), ) world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index()) # Creating perspective cam X_WB = xyz_rpy_deg( [ 1.6 / scaling + trans_x, -1.6 / scaling + trans_y, 1.2 / scaling + trans_z ], [-120, 0, 45], ) sensor_perspective = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating top cam X_WB = xyz_rpy_deg([0 + trans_x, 0 + trans_y, 2.2 / scaling + trans_z], [-180, 0, -90]) sensor_top = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating front cam X_WB = xyz_rpy_deg([2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, 90]) sensor_front = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating side cam X_WB = xyz_rpy_deg([0 + trans_x, 2.2 / scaling + trans_y, 0 + trans_z], [-90, 0, 180]) sensor_side = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating back cam X_WB = xyz_rpy_deg([-2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, -90]) sensor_back = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) DrakeVisualizer.AddToBuilder(builder, scene_graph) # Remove gravity to avoid extra movements of the model when running the simulation plant.gravity_field().set_gravity_vector( np.array([0, 0, 0], dtype=np.float64)) # Switch off collisions to avoid problems with random positions collision_filter_manager = scene_graph.collision_filter_manager() model_inspector = scene_graph.model_inspector() geometry_ids = GeometrySet(model_inspector.GetAllGeometryIds()) collision_filter_manager.Apply( CollisionFilterDeclaration().ExcludeWithin(geometry_ids)) plant.Finalize() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() dofs = plant.num_actuated_dofs() if dofs != plant.num_positions(): raise ValueError( "Error on converted model: Num positions is not equal to num actuated dofs." ) if pose_directory == "random_pose": joint_positions = [0] * dofs for joint_name, pose in random_poses.items(): # check if NaN if pose != pose: pose = 0 # drake will add '_joint' when there's a name collision if plant.HasJointNamed(joint_name): joint = plant.GetJointByName(joint_name) else: joint = plant.GetJointByName(joint_name + "_joint") joint_positions[joint.position_start()] = pose sim_plant_context = plant.GetMyContextFromRoot( simulator.get_mutable_context()) plant.get_actuation_input_port(model).FixValue(sim_plant_context, np.zeros((dofs, 1))) plant.SetPositions(sim_plant_context, model, joint_positions) simulator.AdvanceTo(1) generate_images_and_iou(simulator, sensor_perspective, test_specific_temp_directory, pose_directory, 1) generate_images_and_iou(simulator, sensor_top, test_specific_temp_directory, pose_directory, 2) generate_images_and_iou(simulator, sensor_front, test_specific_temp_directory, pose_directory, 3) generate_images_and_iou(simulator, sensor_side, test_specific_temp_directory, pose_directory, 4) generate_images_and_iou(simulator, sensor_back, test_specific_temp_directory, pose_directory, 5)