def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
geom = QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
self.assertTrue(geom.get_frame_id().is_valid())
help="Duration to run each sim.",
default=4.0)
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1]))
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 visualization in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
QuadrotorGeometry.AddToBuilder(builder, plant.get_output_port(0), scene_graph)
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
for i in range(args.trials):
def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)