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