def CreateManipStationPlanRunnerDiagram(station,
kuka_plans,
gripper_setpoint_list,
print_period=1.0):
builder = DiagramBuilder()
iiwa_controller = IiwaController(station, print_period=print_period)
builder.AddSystem(iiwa_controller)
plan_scheduler = PlanScheduler(kuka_plans, gripper_setpoint_list)
builder.AddSystem(plan_scheduler)
builder.Connect(plan_scheduler.iiwa_plan_output_port,
iiwa_controller.plan_input_port)
builder.ExportInput(iiwa_controller.iiwa_position_input_port,
"iiwa_position")
builder.ExportInput(iiwa_controller.iiwa_velocity_input_port,
"iiwa_velocity")
builder.ExportOutput(iiwa_controller.iiwa_position_command_output_port,
"iiwa_position_and_torque_command")
builder.ExportOutput(plan_scheduler.hand_setpoint_output_port,
"gripper_setpoint")
builder.ExportOutput(plan_scheduler.gripper_force_limit_output_port,
"force_limit")
plan_runner = builder.Build()
plan_runner.set_name("Plan Runner")
return plan_runner
def DepthCameraDemoSystem():
builder = DiagramBuilder()
# Create the physics engine + scene graph.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Add a single object into it.
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf"))
# Add a rendering engine
renderer = "my_renderer"
scene_graph.AddRenderer(renderer,
MakeRenderEngineVtk(RenderEngineVtkParams()))
plant.Finalize()
# Add a visualizer just to help us see the object.
use_meshcat = False
if use_meshcat:
meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# Add a camera to the environment.
pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5])
properties = DepthCameraProperties(width=640,
height=480,
fov_y=np.pi / 4.0,
renderer_name=renderer,
z_near=0.1,
z_far=10.0)
camera = builder.AddSystem(
RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=pose,
properties=properties,
show_window=False))
camera.set_name("rgbd_sensor")
builder.Connect(scene_graph.get_query_output_port(),
camera.query_object_input_port())
# Export the camera outputs
builder.ExportOutput(camera.color_image_output_port(), "color_image")
builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image")
# Add a system to convert the camera output into a point cloud
to_point_cloud = builder.AddSystem(
DepthImageToPointCloud(camera_info=camera.depth_camera_info(),
fields=BaseField.kXYZs | BaseField.kRGBs))
builder.Connect(camera.depth_image_32F_output_port(),
to_point_cloud.depth_image_input_port())
builder.Connect(camera.color_image_output_port(),
to_point_cloud.color_image_input_port())
# Export the point cloud output.
builder.ExportOutput(to_point_cloud.point_cloud_output_port(),
"point_cloud")
diagram = builder.Build()
diagram.set_name("depth_camera_demo_system")
return diagram
def make_diagram():
# Use a nested function to ensure that all locals get garbage
# collected quickly.
# Construct a trivial plant and ID controller.
# N.B. We explicitly do *not* add this plant to the diagram.
controller_plant = MultibodyPlant(time_step=0.002)
controller_plant.Finalize()
builder = DiagramBuilder()
controller = builder.AddSystem(
InverseDynamicsController(
controller_plant,
kp=[],
ki=[],
kd=[],
has_reference_acceleration=False,
)
)
# Forward ports for ease of testing.
builder.ExportInput(
controller.get_input_port_estimated_state(), "x_estimated")
builder.ExportInput(
controller.get_input_port_desired_state(), "x_desired")
builder.ExportOutput(controller.get_output_port_control(), "u")
diagram = builder.Build()
return diagram
def test_diagram_fan_out(self):
builder = DiagramBuilder()
adder = builder.AddSystem(Adder(6, 1))
adder.set_name("adder")
builder.ExportOutput(adder.get_output_port())
in0_index = builder.ExportInput(adder.get_input_port(0), "in0")
in1_index = builder.ExportInput(adder.get_input_port(1), "in1")
# Exercise ConnectInput overload bindings, with and without argument
# names.
builder.ConnectInput(in0_index, adder.get_input_port(2))
builder.ConnectInput("in1", adder.get_input_port(3))
builder.ConnectInput(diagram_port_name="in0",
input=adder.get_input_port(4))
builder.ConnectInput(diagram_port_index=in1_index,
input=adder.get_input_port(5))
diagram = builder.Build()
diagram.set_name("fan_out_diagram")
graph = diagram.GetGraphvizString()
# Check the desired input topology is in the graph.
self.assertRegex(graph, "_u0 -> .*:u0")
self.assertRegex(graph, "_u1 -> .*:u1")
self.assertRegex(graph, "_u0 -> .*:u2")
self.assertRegex(graph, "_u1 -> .*:u3")
self.assertRegex(graph, "_u0 -> .*:u4")
self.assertRegex(graph, "_u1 -> .*:u5")
def __init__(self, num_inputs, size):
Diagram.__init__(self)
builder = DiagramBuilder()
adder = Adder(num_inputs, size)
builder.AddSystem(adder)
builder.ExportOutput(adder.get_output_port(0))
for i in range(num_inputs):
builder.ExportInput(adder.get_input_port(i))
builder.BuildInto(self)
def make_diagram():
builder = DiagramBuilder()
adder1 = builder.AddNamedSystem("adder1", Adder(2, 2))
adder2 = builder.AddNamedSystem("adder2", Adder(1, 2))
builder.Connect(adder1.get_output_port(), adder2.get_input_port())
builder.ExportInput(adder1.get_input_port(0), "in0")
builder.ExportInput(adder1.get_input_port(1), "in1")
builder.ExportOutput(adder2.get_output_port(), "out")
diagram = builder.Build()
return adder1, adder2, diagram
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = np.array([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2) # Test the BasicVector overload.
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
def test_diagram_simulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
self.assertTrue(builder.empty())
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
self.assertFalse(builder.empty())
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
self.assertEqual(
builder.GetMutableSystems(),
[adder0, adder1, integrator])
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
self.assertEqual(
diagram.GetSystems(),
[adder0, adder1, integrator])
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
# Test binding for PrintSimulatorStatistics
PrintSimulatorStatistics(simulator)
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial)
+ xc_initial)
self.assertTrue(np.allclose(xc, xc_expected))
class DrakeSimDiagram(Diagram):
def __init__(self, config):
Diagram.__init__(self)
dt = config["mbp_dt"]
self._builder = DiagramBuilder()
self._mbp, self._sg = AddMultibodyPlantSceneGraph(self._builder, dt)
self._finalize_functions = []
self._finalized = False
self._rgbd_sensors = dict()
self._renderer_name = None
# === Property accessors ========================================
@property
def mbp(self):
return self._mbp
@property
def sg(self):
return self._sg
@property
def builder(self):
return self._builder
@property
def finalize_functions(self):
return self._finalize_functions
@property
def rgbd_sensors(self):
return self._rgbd_sensors
# === Add visualizers ===========================================
def connect_to_meshcat(self):
self._meshcat = ConnectMeshcatVisualizer(
self._builder,
scene_graph=self._sg,
zmq_url="tcp://127.0.0.1:6000",
draw_period=1)
return self._meshcat
def connect_to_drake_visualizer(self):
self._drake_viz = DrakeVisualizer.AddToBuilder(builder=self._builder,
scene_graph=self._sg)
return self._drake_viz
# === Add Cameras ===============================================
def add_rgbd_sensors_from_config(self, config):
if not config["rgbd_sensors"]["enabled"]:
return
for camera_name, sensor_config in iteritems(
config["rgbd_sensors"]["sensor_list"]):
self.add_rgbd_sensor(camera_name, sensor_config)
def add_rgbd_sensor(self, camera_name, sensor_config):
"""
Adds Rgbd camera to the diagram
"""
builder = self._builder
if self._renderer_name is None:
self._renderer_name = "vtk_renderer"
self._sg.AddRenderer(self._renderer_name,
MakeRenderEngineVtk(RenderEngineVtkParams()))
width = sensor_config['width']
height = sensor_config['height']
fov_y = sensor_config['fov_y']
z_near = sensor_config['z_near']
z_far = sensor_config['z_far']
# This is in right-down-forward convention
X_W_camera = transform_from_dict(sensor_config)
color_camera = ColorRenderCamera(
RenderCameraCore(self._renderer_name,
CameraInfo(width, height, fov_y),
ClippingRange(z_near, z_far), RigidTransform()),
False)
depth_camera = DepthRenderCamera(color_camera.core(),
DepthRange(z_near, z_far))
# add camera system
camera = builder.AddSystem(
RgbdSensor(parent_id=self._sg.world_frame_id(),
X_PB=X_W_camera,
color_camera=color_camera,
depth_camera=depth_camera))
builder.Connect(self._sg.get_query_output_port(),
camera.query_object_input_port())
self._rgbd_sensors[camera_name] = camera
# === Finalize the completed diagram ============================
def finalize(self):
self._mbp.Finalize()
self._finalized = True
for func in self._finalize_functions:
func()
self._builder.ExportOutput(self._sg.get_pose_bundle_output_port(),
"pose_bundle")
self._builder.ExportOutput(self._mbp.get_contact_results_output_port(),
"contact_results")
self._builder.ExportOutput(self._mbp.get_state_output_port(),
"plant_continuous_state")
self._builder.ExportOutput(self._mbp.get_geometry_poses_output_port(),
"geometry_poses")
self._builder.ExportInput(
self._mbp.get_applied_spatial_force_input_port(), "spatial_input")
self._builder.ExportOutput(self._mbp.get_body_poses_output_port(),
"body_poses")
self._builder.BuildInto(self)
def is_finalized(self):
return self._finalized
# === Camera helpers ============================================
def get_image_observations_single_sensor(self, sensor_name, context):
assert self.is_finalized()
sensor = self._rgbd_sensors[sensor_name]
sensor_context = self.GetSubsystemContext(sensor, context)
rgb = sensor.color_image_output_port().Eval(sensor_context)
depth_32F = sensor.depth_image_32F_output_port().Eval(sensor_context)
depth_16U = sensor.depth_image_16U_output_port().Eval(sensor_context)
label = sensor.label_image_output_port().Eval(sensor_context)
return {
'rgb': np.copy(rgb.data[:, :, :3]),
'depth_32F': np.copy(depth_32F.data).squeeze(),
'depth_16U': np.copy(depth_16U.data).squeeze(),
'label': np.copy(label.data).squeeze()
}
def get_image_observations(self, context):
image_dict = dict()
for sensor_name in self._rgbd_sensors:
image_dict[
sensor_name] = self.get_image_observations_single_sensor(
sensor_name, context)
return image_dict
def get_label_db(self):
"""
Builds database that associates bodies and labels
:return: TinyDB database
"""
db = TinyDB(storage=MemoryStorage)
for i in range(self._mbp.num_bodies()):
body = self._mbp.get_body(BodyIndex(i))
model_instance_id = int(body.model_instance())
body_name = body.name()
model_name = self._mbp.GetModelInstanceName(body.model_instance())
entry = {
'label': i,
'model_instance_id': model_instance_id,
'body_name': body_name,
'model_name': model_name
}
db.insert(entry)
return db
import matplotlib.pyplot as plt
from pydrake.systems.drawing import plot_system_graphviz
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import Adder
builder = DiagramBuilder()
size = 1
adders = [
builder.AddSystem(Adder(1, size)),
builder.AddSystem(Adder(1, size)),
]
for i, adder in enumerate(adders):
adder.set_name("adders[{}]".format(i))
builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0))
builder.ExportInput(adders[0].get_input_port(0))
builder.ExportOutput(adders[1].get_output_port(0))
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram)
plt.show()
