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 test_decomposition_controller_like_workflow(self):
"""Tests subgraphs (post-finalize) for decomposition, with a
scene-graph. Also shows a workflow of replacing joints / welding
joints."""
builder = DiagramBuilder()
# N.B. I (Eric) am using ManipulationStation because it's currently
# the simplest way to get a complex scene in pydrake.
station = ManipulationStation(time_step=0.001)
station.SetupManipulationClassStation()
station.Finalize()
builder.AddSystem(station)
plant = station.get_multibody_plant()
scene_graph = station.get_scene_graph()
iiwa = plant.GetModelInstanceByName("iiwa")
wsg = plant.GetModelInstanceByName("gripper")
if VISUALIZE:
print("test_decomposition_controller_like_workflow")
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
diagram = builder.Build()
# Set the context with which things should be computed.
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7]
ndof = 7
q_wsg = [-0.03, 0.03]
plant.SetPositions(context, iiwa, q_iiwa)
plant.SetPositions(context, wsg, q_wsg)
# Add copy of only the IIWA to a control diagram.
control_builder = DiagramBuilder()
control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0))
# N.B. This has the same scene, but with all joints outside of the
# IIWA frozen
# TODO(eric.cousineau): Re-investigate weird "build_with_no_control"
# behavior (with scene graph) with default conditions and time_step=0
# - see Anzu commit 2cf08cfc3.
to_control = mut.add_plant_with_articulated_subset_to(
plant_src=plant,
scene_graph_src=scene_graph,
articulated_models_src=[iiwa],
context_src=context,
plant_dest=control_plant)
self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap)
control_iiwa = to_control.model_instances[iiwa]
control_plant.Finalize()
self.assertEqual(control_plant.num_positions(),
plant.num_positions(iiwa))
kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500])
kd = np.sqrt(2 * kp)
ki = np.zeros(7)
controller = control_builder.AddSystem(
InverseDynamicsController(robot=control_plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=False))
# N.B. Rather than use model instances for direct correspence, we could
# use the mappings themselves within a custom system.
control_builder.Connect(
control_plant.get_state_output_port(control_iiwa),
controller.get_input_port_estimated_state())
control_builder.Connect(
controller.get_output_port_control(),
control_plant.get_actuation_input_port(control_iiwa))
# Control to having the elbow slightly bent.
q_iiwa_final = np.zeros(7)
q_iiwa_final[3] = -np.pi / 2
t_start = 0.
t_end = 1.
nx = 2 * ndof
def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx):
s = (t - t_start) / (t_end - t_start)
ds = 1 / (t_end - t_start)
q = q_iiwa + s * (q_iiwa_final - q_iiwa)
v = ds * (q_iiwa_final - q_iiwa)
x = np.hstack((q, v))
return x
interpolator = control_builder.AddSystem(
FunctionSystem(q_desired_interpolator))
control_builder.Connect(interpolator.get_output_port(0),
controller.get_input_port_desired_state())
control_diagram = control_builder.Build()
control_d_context = control_diagram.CreateDefaultContext()
control_context = control_plant.GetMyContextFromRoot(control_d_context)
to_control.copy_state(context, control_context)
util.compare_frame_poses(plant, context, control_plant,
control_context, "iiwa_link_0", "iiwa_link_7")
util.compare_frame_poses(plant, context, control_plant,
control_context, "body", "left_finger")
from_control = to_control.inverse()
def viz_monitor(control_d_context):
# Simulate control, visualizing in original diagram.
assert (control_context is
control_plant.GetMyContextFromRoot(control_d_context))
from_control.copy_state(control_context, context)
d_context.SetTime(control_d_context.get_time())
diagram.Publish(d_context)
return EventStatus.DidNothing()
simulator = Simulator(control_diagram, control_d_context)
simulator.Initialize()
if VISUALIZE:
simulator.set_monitor(viz_monitor)
simulator.set_target_realtime_rate(1.)
simulator.AdvanceTo(t_end)
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...")