def _add_renderer(self):
renderer_name = "renderer"
self.scenegraph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams()))
# Add a camera
depth_camera = DepthRenderCamera(
RenderCameraCore(
renderer_name,
CameraInfo(width=640, height=480, fov_y=np.pi/4),
ClippingRange(0.01, 10.0),
RigidTransform()),
DepthRange(0.01,10.0)
)
world_id = self.plant.GetBodyFrameIdOrThrow(self.plant.world_body().index())
X_WB = xyz_rpy_deg([4,0,0],[-90,0,90])
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
self.builder.AddSystem(sensor)
self.builder.Connect(self.scenegraph.get_query_output_port(), sensor.query_object_input_port())
def create_camera(builder, world_id, X_WB, depth_camera, scene_graph):
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
builder.AddSystem(sensor)
builder.Connect(scene_graph.get_query_output_port(),
sensor.query_object_input_port())
return sensor
# Now a camera with color and depth (values chosen arbitrarily)
# So here we leverage the renderer attached to the scene graph to power a camera for our robot
depth_camera = DepthRenderCamera(
RenderCameraCore(renderer_name,
CameraInfo(width=640, height=480, fov_y=np.pi / 4),
ClippingRange(0.01, 10.0), RigidTransform()),
DepthRange(0.01, 10.0))
world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index())
X_WB = xyz_rpy_deg([4, 0, 0], [-90, 0, 90])
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
builder.AddSystem(sensor)
builder.Connect(scene_graph.get_query_output_port(),
sensor.query_object_input_port())
# So here we are adding the drake visualizer into the mix.
# IMPORTANT: Need to actually run "drake visualizer" from the build.
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# If we want meshcat instead, can run this line
# Or can swap out the "new" and instead connect to an existing meshcat-server instance
# meshcat_vis = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url="new", open_browser=False)
plant.Finalize()
diagram = builder.Build()
# Creating top level context and extracting subsystem contexts
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyMutableContextFromRoot(diagram_context)
plant.GetModelInstanceByName(camera.reference_object))
for idx in body_indices:
body = plant.get_body(idx)
if body.name() == camera.reference_object_frame:
# we found the frame we want
frame_id = plant.GetBodyFrameIdOrThrow(idx)
if frame_id is None:
frame_id = world_id
camera_instance = RgbdSensor(frame_id,
X_PB=camera.pose,
color_properties=color_prop,
depth_properties=depth_prop)
camera_instance.set_name(camera.name)
builder.AddSystem(camera_instance)
builder.Connect(scene_graph.get_query_output_port(),
camera_instance.query_object_input_port())
#add to the dictionary of cameras, with no image associated for now
camera_images_rgb[camera.name] = np.zeros([480, 640, 4])
camera_images_depth[camera.name] = np.zeros([480, 640, 1])
color_info = camera_instance.color_camera_info()
depth_info = camera_instance.depth_camera_info()
cloud_generator_instance = DepthImageToPointCloud(depth_info)
builder.AddSystem(cloud_generator_instance)
builder.Connect(camera_instance.depth_image_32F_output_port(),
cloud_generator_instance.depth_image_input_port())
image_to_lcm_image_array = builder.AddSystem(ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
cam_port = (image_to_lcm_image_array.DeclareImageInputPort[
PixelType.kRgba8U]("camera_" + str(0)))
builder.Connect(camera_instance.color_image_output_port(), cam_port)