def construct_single(parent_id, X_PB): depth_camera = DepthRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, depth_camera=depth_camera)
def construct(parent_id, X_PB): color_camera = ColorRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), False) depth_camera = DepthRenderCamera(color_camera.core(), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, color_camera=color_camera, depth_camera=depth_camera)
def test_rgbd_sensor_registration(self): X_PC = RigidTransform(p=[1, 2, 3]) station = ManipulationStation(time_step=0.001) station.SetupManipulationClassStation() plant = station.get_multibody_plant() color_camera = ColorRenderCamera( RenderCameraCore("renderer", CameraInfo(10, 10, np.pi / 4), ClippingRange(0.1, 10.0), RigidTransform()), False) depth_camera = DepthRenderCamera(color_camera.core(), DepthRange(0.1, 9.5)) station.RegisterRgbdSensor("single_sensor", plant.world_frame(), X_PC, depth_camera) station.RegisterRgbdSensor("dual_sensor", plant.world_frame(), X_PC, color_camera, depth_camera) station.Finalize() camera_poses = station.GetStaticCameraPosesInWorld() # The three default cameras plus the two just added. self.assertEqual(len(camera_poses), 5) self.assertTrue("single_sensor" in camera_poses) self.assertTrue("dual_sensor" in camera_poses)
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
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa_2), X_AB=xyz_rpy_deg([0, 1, 0], [0, 0, 0])) # Adding the renderer / vtk stuff? renderer_name = "renderer" scene_graph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # 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
def perform_iou_testing(model_file, test_specific_temp_directory, pose_directory): random_poses = {} # Read camera translation calculated and applied on gazebo # we read the random positions file as it contains everything: with open( test_specific_temp_directory + "/pics/" + pose_directory + "/poses.txt", "r") as datafile: for line in datafile: if line.startswith("Translation:"): line_split = line.split(" ") # we make the value negative since gazebo moved the robot # and in drakewe move the camera trans_x = float(line_split[1]) trans_y = float(line_split[2]) trans_z = float(line_split[3]) elif line.startswith("Scaling:"): line_split = line.split(" ") scaling = float(line_split[1]) else: line_split = line.split(" ") if line_split[1] == "nan": random_poses[line_split[0][:-1]] = 0 else: random_poses[line_split[0][:-1]] = float(line_split[1]) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) model = make_parser(plant).AddModelFromFile(model_file) model_bodies = me.get_bodies(plant, {model}) frame_W = plant.world_frame() frame_B = model_bodies[0].body_frame() if len(plant.GetBodiesWeldedTo(plant.world_body())) < 2: plant.WeldFrames(frame_W, frame_B, X_PC=plant.GetDefaultFreeBodyPose(frame_B.body())) # Creating cameras: renderer_name = "renderer" scene_graph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # N.B. These properties are chosen arbitrarily. depth_camera = DepthRenderCamera( RenderCameraCore( renderer_name, CameraInfo( width=960, height=540, focal_x=831.382036787, focal_y=831.382036787, center_x=480, center_y=270, ), ClippingRange(0.01, 10.0), RigidTransform(), ), DepthRange(0.01, 10.0), ) world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index()) # Creating perspective cam X_WB = xyz_rpy_deg( [ 1.6 / scaling + trans_x, -1.6 / scaling + trans_y, 1.2 / scaling + trans_z ], [-120, 0, 45], ) sensor_perspective = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating top cam X_WB = xyz_rpy_deg([0 + trans_x, 0 + trans_y, 2.2 / scaling + trans_z], [-180, 0, -90]) sensor_top = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating front cam X_WB = xyz_rpy_deg([2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, 90]) sensor_front = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating side cam X_WB = xyz_rpy_deg([0 + trans_x, 2.2 / scaling + trans_y, 0 + trans_z], [-90, 0, 180]) sensor_side = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) # Creating back cam X_WB = xyz_rpy_deg([-2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z], [-90, 0, -90]) sensor_back = create_camera(builder, world_id, X_WB, depth_camera, scene_graph) DrakeVisualizer.AddToBuilder(builder, scene_graph) # Remove gravity to avoid extra movements of the model when running the simulation plant.gravity_field().set_gravity_vector( np.array([0, 0, 0], dtype=np.float64)) # Switch off collisions to avoid problems with random positions collision_filter_manager = scene_graph.collision_filter_manager() model_inspector = scene_graph.model_inspector() geometry_ids = GeometrySet(model_inspector.GetAllGeometryIds()) collision_filter_manager.Apply( CollisionFilterDeclaration().ExcludeWithin(geometry_ids)) plant.Finalize() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() dofs = plant.num_actuated_dofs() if dofs != plant.num_positions(): raise ValueError( "Error on converted model: Num positions is not equal to num actuated dofs." ) if pose_directory == "random_pose": joint_positions = [0] * dofs for joint_name, pose in random_poses.items(): # check if NaN if pose != pose: pose = 0 # drake will add '_joint' when there's a name collision if plant.HasJointNamed(joint_name): joint = plant.GetJointByName(joint_name) else: joint = plant.GetJointByName(joint_name + "_joint") joint_positions[joint.position_start()] = pose sim_plant_context = plant.GetMyContextFromRoot( simulator.get_mutable_context()) plant.get_actuation_input_port(model).FixValue(sim_plant_context, np.zeros((dofs, 1))) plant.SetPositions(sim_plant_context, model, joint_positions) simulator.AdvanceTo(1) generate_images_and_iou(simulator, sensor_perspective, test_specific_temp_directory, pose_directory, 1) generate_images_and_iou(simulator, sensor_top, test_specific_temp_directory, pose_directory, 2) generate_images_and_iou(simulator, sensor_front, test_specific_temp_directory, pose_directory, 3) generate_images_and_iou(simulator, sensor_side, test_specific_temp_directory, pose_directory, 4) generate_images_and_iou(simulator, sensor_back, test_specific_temp_directory, pose_directory, 5)