def test_test_temp_directory(self): temp_dir = mut.temp_directory() # We'll simply confirm that the path *starts* with the TEST_TMPDIR and # that it exists. We'll assume that it otherwise has the documented # properties. self.assertTrue(temp_dir.startswith(os.environ.get('TEST_TMPDIR'))) self.assertTrue(os.path.exists(temp_dir))
def run_pendulum_example(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf")) plant.Finalize() T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = ConnectPlanarSceneGraphVisualizer( builder, scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]) if args.playback: visualizer.start_recording() 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.get_actuation_input_port().FixValue( plant_context, np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.AdvanceTo(args.duration) if args.playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() # Playback the recording and save the output. ani.save("{}/pend_playback.mp4".format(temp_directory()), fps=30)
def setUp(self): self._expected_non_native_modules = [ # A standalone 'import pydrake' should not trigger native code. "pydrake", # Another example of a module we'd want to be non-native would be # pydrake.lcmtypes, but we don't have such a module (yet). ] self._temp_dir = temp_directory()
def test_GenerateSDPA(self): prog = mp.MathematicalProgram() X = prog.NewSymmetricContinuousVariables(2) prog.AddPositiveSemidefiniteConstraint(X) prog.AddLinearCost(X[0, 0] + X[1, 1]) prog.AddBoundingBoxConstraint(1, 1, X[0, 1]) file_name = temp_directory() + "/sdpa" self.assertTrue(GenerateSDPA(prog=prog, file_name=file_name)) self.assertTrue(os.path.exists(file_name + ".dat-s"))
def test_write_to_file(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddLinearConstraint(x[0] + x[1] == 1) prog.AddQuadraticCost(x[0] * x[0] + x[1] * x[1]) solver = GurobiSolver() file_name = temp_directory() + "/gurobi.mps" options = mp.SolverOptions() options.SetOption(solver.id(), "GRBwrite", file_name) result = solver.Solve(prog, None, options) self.assertTrue(os.path.exists(file_name))
def test_compute_iis(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddBoundingBoxConstraint(1, np.inf, x) prog.AddLinearConstraint(x[0] + x[1] == 1) solver = GurobiSolver() ilp_file_name = temp_directory() + "/gurobi.ilp" options = mp.SolverOptions() options.SetOption(solver.id(), "GRBwrite", ilp_file_name) options.SetOption(solver.id(), "GRBcomputeIIS", 1) result = solver.Solve(prog, None, options) self.assertTrue(os.path.exists(ilp_file_name))
def run_manipulation_example(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupManipulationClassStation() 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. T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, T_VW=T_VW, 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)) if args.playback: visualizer.start_recording() 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.AdvanceTo(args.duration) if args.playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() # Playback the recording and save the output. ani.save("{}/manip_playback.mp4".format(temp_directory()), fps=30)
def plot_graphviz(dot_text): """Renders a DOT graph in matplotlib.""" # @ref https://stackoverflow.com/a/18522941/7829525 # Tried (reason ignored): pydotplus (`pydot` works), networkx # (`read_dot` does not work robustly?), pygraphviz (coupled with # `networkx`). g = pydot.graph_from_dot_data(dot_text) if isinstance(g, list): # Per Ioannis's follow-up comment in the above link, in pydot >= 1.2.3 # `graph_from_dot_data` returns a list of graphs. # Handle this case for now. assert len(g) == 1 g = g[0] f = NamedTemporaryFile(suffix='.png', dir=temp_directory()) g.write_png(f.name) plt.axis('off') return plt.imshow(plt.imread(f.name), aspect="equal")
def test_temp_directory(self): self.assertEqual(os.environ.get('TEST_TMPDIR'), mut.temp_directory())