def _setResolution(self, resolution): """ INTERNAL METHOD, sets the resolution for the camera. The fov of the camera has to be setted beforehand Parameters: resolution - The resolution of the camera (CameraResolution type) """ try: with self.resolution_lock: assert isinstance(resolution, CameraResolution) assert self.hfov is not None and self.vfov is not None self.resolution = resolution self.projection_matrix = pybullet.computeProjectionMatrix( left=-math.tan(math.pi * self.hfov / 360.0) * self.near_plane, right=math.tan(math.pi * self.hfov / 360.0) * self.near_plane, bottom=-math.tan(math.pi * self.vfov / 360.0) * self.near_plane, top=math.tan(math.pi * self.vfov / 360.0) * self.near_plane, nearVal=self.near_plane, farVal=self.far_plane, physicsClientId=self.physics_client) except AssertionError as e: print("Cannot set camera resolution: " + str(e))
def get_camera_image(self): """Get the camera image of the scene Returns ------- tuple Three arrays corresponding to rgb, depth, and segmentation image. """ upAxisIndex = 2 camDistance = 500 pixelWidth = 350 pixelHeight = 700 camTargetPos = [0, 80, 0] far = camDistance near = -far view_matrix = p.computeViewMatrixFromYawPitchRoll( camTargetPos, camDistance, 0, 90, 0, upAxisIndex) projection_matrix = p.computeProjectionMatrix(-90, 60, 150, -150, near, far) # Get depth values using the OpenGL renderer width, height, rgbImg, depthImg, segImg = p.getCameraImage( pixelWidth, pixelHeight, view_matrix, projection_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) return rgbImg, depthImg, segImg
def __init__(self, viewMatrixList=((0,0,3), (0,0,0), (0,1,0)), projectionMatrixList=(45.0, 1.0, 0.1, 3.1), width=224, height=224): self._viewMatrixList = viewMatrixList self._projectionMatrixList = projectionMatrixList self._width = width self._height = height self._viewMatrix = p.computeViewMatrix(self._viewMatrixList[0], self._viewMatrixList[1],self._viewMatrixList[2],) # cameraEyePosition=[0, 0, 3], cameraTargetPosition=[0, 0, 0], cameraUpVector=[0, 1, 0] #self._projectionMatrix = p.computeProjectionMatrixFOV(self._projectionMatrixList[0], self._projectionMatrixList[1], self._projectionMatrixList[2], self._projectionMatrixList[3]) # fov=45.0,aspect=1.0, nearVal=0.1, farVal=3.1 self._projectionMatrix = p.computeProjectionMatrix(-0.5,0.5,-0.5,0.5,0.1,40.0) # Screen left, right, bottom, top, near plane distance, far plane distance // Screen LRTB is for image size from origin (ie. -x,+x,-y,+y)
def take_1photo(): imageFrame = [] width = 3000 height = 3000 view_matrix = p.computeViewMatrix([0, 0, 50], [0, 0, 0.0], [1, 0, 0]) projection_matrix = p.computeProjectionMatrix(12, -12, 12, -12, 48.0, 51.1) width, height, rgbaImg, depthImg, segImg = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL) img, cubePositions, patchPositions = boxDetection2411(rgbaImg,width,height) imageFrame.append(img) return imageFrame, cubePositions, patchPositions
def _projection_matrix(self): width = self.width height = self.height near = self.near far = self.far fx = self.fx fy = self.fy cx = self.cx cy = self.cy # TODO(yl): verify this left = cx * near / -fx top = cy * near / fy right = -(width - cx) * near / -fx bottom = -(height - cy) * near / fy # Note pybullet returns the projection matrix in column major # To convert it into the convention used in Python, do # np.asarray(projectionMatrix).reshape((4,4)).T projectionMatrix = pybullet.computeProjectionMatrix( left, right, bottom, top, near, far ) return projectionMatrix
def _setResolution(self, resolution): """ INTERNAL METHOD, sets the resolution for the camera. The fov of the camera has to be setted beforehand Parameters: resolution - The resolution of the camera (CameraResolution type) """ try: with self.resolution_lock: assert isinstance(resolution, CameraResolution) assert self.hfov is not None and self.vfov is not None self.resolution = resolution self.projection_matrix = pybullet.computeProjectionMatrix( left=-math.tan(math.pi*self.hfov/360.0)*self.near_plane, right=math.tan(math.pi*self.hfov/360.0)*self.near_plane, bottom=-math.tan(math.pi*self.vfov/360.0)*self.near_plane, top=math.tan(math.pi*self.vfov/360.0)*self.near_plane, nearVal=self.near_plane, farVal=self.far_plane, physicsClientId=self.getPhysicsClientId()) self.intrinsic_matrix = [ self.projection_matrix[0]*self.resolution.width/2.0, 0.0, (1-self.projection_matrix[8])*self.resolution.width/2.0, 0.0, self.projection_matrix[5]*self.resolution.height/2.0, (1-self.projection_matrix[9])*self.resolution.height/2.0, 0.0, 0.0, 1.0] except AssertionError as e: raise pybullet.error("Cannot set camera resolution: " + str(e))
def kinova_pusher_camera(camera): camera.CameraViewMatrix = p.computeViewMatrixFromYawPitchRoll( [0.7, 0.1, -0.00], 1, 90, -45, 0, 2) camera.CameraProjMatrix = p.computeProjectionMatrix( -0.5000, 0.5000, -0.5000, 1.5000, 1.0000, 6.0000)
def __init__(self, config): if 'robot' not in config: config['robot'] = 'ur5' if 'pos_candidate' not in config: config['pos_candidate'] = None if 'perfect_grasp' not in config: config['perfect_grasp'] = False if 'perfect_place' not in config: config['perfect_place'] = False if 'workspace_check' not in config: config['workspace_check'] = 'box' if 'in_hand_size' not in config: config['in_hand_size'] = 24 if 'in_hand_mode' not in config: config['in_hand_mode'] = 'sub' if 'num_random_objects' not in config: config['num_random_objects'] = 0 if 'check_random_obj_valid' not in config: config['check_random_obj_valid'] = False seed = config['seed'] workspace = config['workspace'] max_steps = config['max_steps'] obs_size = config['obs_size'] fast_mode = config['fast_mode'] render = config['render'] action_sequence = config['action_sequence'] simulate_grasp = config['simulate_grasp'] pos_candidate = config['pos_candidate'] perfect_grasp = config['perfect_grasp'] perfect_place = config['perfect_place'] robot = config['robot'] workspace_check = config['workspace_check'] in_hand_size = config['in_hand_size'] in_hand_mode = config['in_hand_mode'] num_random_objects = config['num_random_objects'] check_random_obj_valid = config['check_random_obj_valid'] super(PyBulletEnv, self).__init__(seed, workspace, max_steps, obs_size, action_sequence, pos_candidate, in_hand_size, in_hand_mode) # Connect to pybullet and add data files to path if render: self.client = pb.connect(pb.GUI) else: self.client = pb.connect(pb.DIRECT) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) self.dynamic = not fast_mode # Environment specific variables self._timestep = 1. / 240. if robot == 'ur5': self.robot = UR5_Simple() elif robot == 'ur5_robotiq': self.robot = UR5_Robotiq() elif robot == 'kuka': self.robot = Kuka() else: raise NotImplementedError # TODO: Move this somewhere it makes sense self.block_original_size = 0.05 self.block_scale_range = (0.6, 0.7) self.min_block_size = self.block_original_size * self.block_scale_range[ 0] self.max_block_size = self.block_original_size * self.block_scale_range[ 1] self.pick_pre_offset = 0.1 self.pick_offset = 0.005 self.place_pre_offset = 0.1 self.place_offset = self.block_scale_range[1] * self.block_original_size # Setup camera parameters workspace_x_offset = (workspace[0][1] - workspace[0][0]) / 2 workspace_y_offset = (workspace[1][1] - workspace[1][0]) / 2 self.view_matrix = pb.computeViewMatrixFromYawPitchRoll( [workspace[0].mean(), workspace[1].mean(), 0], 10, -90, -90, 0, 2) self.proj_matrix = pb.computeProjectionMatrix(-workspace_x_offset, workspace_x_offset, -workspace_y_offset, workspace_y_offset, -10.0, 100.0) # self.view_matrix = pb.computeViewMatrixFromYawPitchRoll([workspace[0].mean(), workspace[1].mean(), 0], 3, -90, -90, 0, 2) # self.proj_matrix = pb.computeProjectionMatrixFOV(5.7, 1, 2, 3.01) # Rest pose for arm rot = pb.getQuaternionFromEuler([0, np.pi, 0]) self.rest_pose = [[0.0, 0.5, 0.5], rot] self.objects = list() self.object_types = {} self.simulate_grasp = simulate_grasp self.perfect_grasp = perfect_grasp self.perfect_place = perfect_place self.workspace_check = workspace_check self.table_id = None self.heightmap = None self.current_episode_steps = 1 self.last_action = None self.last_obj = None self.state = {} self.pb_state = None self.initialize() self.num_random_objects = num_random_objects self.check_random_obj_valid = check_random_obj_valid self.episode_count = 0