def test_system_monitor(self):
x = Variable("x")
sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3])
simulator = Simulator(sys)
def monitor(root_context):
context = sys.GetMyContextFromRoot(root_context)
if context.get_time() >= 1.:
return EventStatus.ReachedTermination(sys, "Time reached")
else:
return EventStatus.DidNothing()
self.assertIsNone(simulator.get_monitor())
simulator.set_monitor(monitor)
self.assertIsNotNone(simulator.get_monitor())
status = simulator.AdvanceTo(2.)
self.assertEqual(
status.reason(),
SimulatorStatus.ReturnReason.kReachedTerminationCondition)
self.assertLess(status.return_time(), 1.1)
simulator.clear_monitor()
self.assertIsNone(simulator.get_monitor())
def test_exploding_iiwa_sim(self):
"""
Shows a simulation of a falling + exploding IIWA which "changes
topology" by being rebuilt without joints.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
iiwa_file = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/"
"iiwa14_spheres_dense_elbow_collision.urdf")
Parser(plant).AddModelFromFile(iiwa_file, "iiwa")
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
plant.Finalize()
# Loosey-goosey - no control.
for model in me.get_model_instances(plant):
util.build_with_no_control(builder, plant, model)
if VISUALIZE:
print("test_exploding_iiwa_sim")
role = Role.kIllustration
DrakeVisualizer.AddToBuilder(
builder, scene_graph, params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
# - Hoist IIWA up in the air.
plant.SetFreeBodyPose(context, plant.GetBodyByName("base"),
RigidTransform([0, 0, 1.]))
# - Set joint velocities to "spin" it in the air.
for joint in me.get_joints(plant):
if isinstance(joint, RevoluteJoint):
me.set_joint_positions(plant, context, joint, 0.7)
me.set_joint_velocities(plant, context, joint, -5.)
def monitor(d_context):
# Stop the simulation once there's any contact.
context = plant.GetMyContextFromRoot(d_context)
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
return EventStatus.ReachedTermination(plant, "Contact")
else:
return EventStatus.DidNothing()
# Forward simulate.
simulator = Simulator(diagram, d_context)
simulator.Initialize()
simulator.set_monitor(monitor)
if VISUALIZE:
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(2.)
# Try to push a bit further.
simulator.clear_monitor()
simulator.AdvanceTo(d_context.get_time() + 0.05)
diagram.Publish(d_context)
# Recreate simulator.
builder_new = DiagramBuilder()
plant_new, scene_graph_new = AddMultibodyPlantSceneGraph(
builder_new, time_step=plant.time_step())
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant, scene_graph))
# Remove all joints; make them floating bodies.
for joint in me.get_joints(plant):
subgraph.remove_joint(joint)
# Remove massless bodies.
# For more info, see: https://stackoverflow.com/a/62035705/7829525
for body in me.get_bodies(plant):
if body is plant.world_body():
continue
if body.default_mass() == 0.:
subgraph.remove_body(body)
# Finalize.
to_new = subgraph.add_to(plant_new, scene_graph_new)
plant_new.Finalize()
if VISUALIZE:
DrakeVisualizer.AddToBuilder(
builder_new,
scene_graph_new,
params=DrakeVisualizerParams(role=role))
ConnectContactResultsToDrakeVisualizer(builder_new, plant_new,
scene_graph_new)
diagram_new = builder_new.Build()
# Remap state.
d_context_new = diagram_new.CreateDefaultContext()
d_context_new.SetTime(d_context.get_time())
context_new = plant_new.GetMyContextFromRoot(d_context_new)
to_new.copy_state(context, context_new)
# Simulate.
simulator_new = Simulator(diagram_new, d_context_new)
simulator_new.Initialize()
diagram_new.Publish(d_context_new)
if VISUALIZE:
simulator_new.set_target_realtime_rate(1.)
simulator_new.AdvanceTo(context_new.get_time() + 2)
if VISUALIZE:
input(" Press enter...")
def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src):
# Show a simulation which "changes state" by being rebuilt.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=1e-3)
subgraph_src = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant_src, scene_graph_src))
to_plant = subgraph_src.add_to(plant, scene_graph)
# Add ground plane.
X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(),
"ground_plane_collision",
CoulombFriction(0.8, 0.3))
plant.Finalize()
# Loosey-goosey - no control.
for model in me.get_model_instances(plant):
util.no_control(builder, plant, model)
if VISUALIZE:
print("do_exploding_iiwa_sim")
role = Role.kIllustration
ConnectDrakeVisualizer(builder, scene_graph, role=role)
ConnectContactResultsToDrakeVisualizer(builder, plant)
diagram = builder.Build()
# Set up context.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
to_plant.copy_state(context_src, context)
# - Hoist IIWA up in the air.
plant.SetFreeBodyPose(context, plant.GetBodyByName("base"),
RigidTransform([0, 0, 1.]))
# - Set joint velocities to "spin" it in the air.
for joint in me.get_joints(plant):
if isinstance(joint, RevoluteJoint):
me.set_joint_positions(plant, context, joint, 0.7)
me.set_joint_velocities(plant, context, joint, -5.)
def monitor(d_context):
# Stop the simulation once there's any contact.
context = plant.GetMyContextFromRoot(d_context)
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
return EventStatus.ReachedTermination(plant, "Contact")
else:
return EventStatus.DidNothing()
# Forward simulate.
simulator = Simulator(diagram, d_context)
simulator.Initialize()
simulator.set_monitor(monitor)
if VISUALIZE:
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(2.)
# Try to push a bit further.
simulator.clear_monitor()
simulator.AdvanceTo(d_context.get_time() + 0.05)
diagram.Publish(d_context)
# Recreate simulator.
builder_new = DiagramBuilder()
plant_new, scene_graph_new = AddMultibodyPlantSceneGraph(
builder_new, time_step=plant.time_step())
subgraph = mut.MultibodyPlantSubgraph(
mut.get_elements_from_plant(plant, scene_graph))
# Remove all joints; make them floating bodies.
for joint in me.get_joints(plant):
subgraph.remove_joint(joint)
# Remove massless bodies.
for body in me.get_bodies(plant):
if body is plant.world_body():
continue
if body.default_mass() == 0.:
subgraph.remove_body(body)
# Finalize.
to_new = subgraph.add_to(plant_new, scene_graph_new)
plant_new.Finalize()
if VISUALIZE:
ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role)
ConnectContactResultsToDrakeVisualizer(builder_new, plant_new)
diagram_new = builder_new.Build()
# Remap state.
d_context_new = diagram_new.CreateDefaultContext()
d_context_new.SetTime(d_context.get_time())
context_new = plant_new.GetMyContextFromRoot(d_context_new)
to_new.copy_state(context, context_new)
# Simulate.
simulator_new = Simulator(diagram_new, d_context_new)
simulator_new.Initialize()
diagram_new.Publish(d_context_new)
if VISUALIZE:
simulator_new.set_target_realtime_rate(1.)
simulator_new.AdvanceTo(context_new.get_time() + 2)
if VISUALIZE:
input(" Press enter...")
