def get_image(camera_pos, target_pos, width=640, height=480, vertical_fov=60.0, near=0.02, far=5.0, segment=False, segment_links=False): # computeViewMatrixFromYawPitchRoll view_matrix = p.computeViewMatrix(cameraEyePosition=camera_pos, cameraTargetPosition=target_pos, cameraUpVector=[0, 0, 1], physicsClientId=CLIENT) projection_matrix = get_projection_matrix(width, height, vertical_fov, near, far) if segment: if segment_links: flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX else: flags = 0 else: flags = p.ER_NO_SEGMENTATION_MASK image = CameraImage(*p.getCameraImage( width, height, viewMatrix=view_matrix, projectionMatrix=projection_matrix, shadow=False, flags=flags, renderer=p.ER_TINY_RENDERER, # p.ER_BULLET_HARDWARE_OPENGL physicsClientId=CLIENT)[2:]) depth = far * near / (far - (far - near) * image.depthPixels) # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/pointCloudFromCameraImage.py # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/getCameraImageTest.py segmented = None if segment: segmented = np.zeros(image.segmentationMaskBuffer.shape + (2, )) for r in range(segmented.shape[0]): for c in range(segmented.shape[1]): pixel = image.segmentationMaskBuffer[r, c] segmented[r, c, :] = demask_pixel(pixel) return CameraImage(image.rgbPixels, depth, segmented)
def set_view_matrix(self, camera_eye_pos, camera_target_pos, camera_up_vec): """ Sets the camera view matrix (OpenGL ModelView Matrix). Args: camera_eye_pos (list or np.array) : Position of the center of the camera. camera_target_pos (list or np.array) : Position of the center of the scene where the camera is looking at camera_up_vec (list or np.array) : Camera up vector. """ self._camera_eye_pos = camera_eye_pos self._camera_target_pos = camera_target_pos self._camera_up_vec = camera_up_vec # use the Pybullet helper function to compute the view matrix view_mat = p.computeViewMatrix(cameraEyePosition=camera_eye_pos, cameraTargetPosition=camera_target_pos, cameraUpVector=camera_up_vec) # Bullet is using column major matrices self._view_mat = np.reshape(view_mat, (4, 4)).transpose()
def _getCameraImage(self): """ INTERNAL METHOD, Computes the OpenGL virtual camera image. The resolution and the projection matrix have to be computed before calling this method, or it will crash Returns: camera_image - The camera image of the OpenGL virtual camera """ _, _, _, _, pos_world, q_world = pybullet.getLinkState( self.robot_model, self.camera_link.getParentIndex(), computeForwardKinematics=False, physicsClientId=self.physics_client) rotation = pybullet.getMatrixFromQuaternion(q_world) forward_vector = [rotation[0], rotation[3], rotation[6]] up_vector = [rotation[2], rotation[5], rotation[8]] camera_target = [ pos_world[0] + forward_vector[0] * 10, pos_world[1] + forward_vector[1] * 10, pos_world[2] + forward_vector[2] * 10 ] view_matrix = pybullet.computeViewMatrix( pos_world, camera_target, up_vector, physicsClientId=self.physics_client) with self.resolution_lock: camera_image = pybullet.getCameraImage( self.resolution.width, self.resolution.height, view_matrix, self.projection_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, flags=pybullet.ER_NO_SEGMENTATION_MASK, physicsClientId=self.physics_client) return camera_image
def render_img(self, left_gripper_joint, left_gripper2_joint, Tce, plot_cam_pose=False, imsize=(214, 214), plot_img=False, show_time_taken=False): if show_time_taken: start = time.time() p.resetJointStatesMultiDof(self.robot, jointIndices=self.joint_ids, targetValues=[[left_gripper_joint], [left_gripper2_joint]], physicsClientId=self.physicsClient) Tec = Tce.inverse() cam_trans, cam_rot = SE32_trans_rot(Tec) cam_rotm = Tec.rotationMatrix() if plot_cam_pose: draw_pose(cam_trans, cam_rot) # Calculate extrinsic matrix # target position is camera frame y axis tip in global frame # up vector is the z axis of camera frame viewMatrix = p.computeViewMatrix( cameraEyePosition=cam_trans, cameraTargetPosition=(cam_rotm.dot(np.array([0, 1, 0]).T) + np.array(cam_trans)).tolist(), cameraUpVector=cam_rotm[:, 2].tolist()) width, height, rgbImg, depthImg, segImg = p.getCameraImage( width=imsize[0], height=imsize[1], viewMatrix=viewMatrix, projectionMatrix=self.projectionMatrix, flags=p.ER_NO_SEGMENTATION_MASK, physicsClientId=self.physicsClient) if show_time_taken: print("time taken for rendering: {}".format(time.time() - start)) if plot_img: plt.imshow(rgbImg[:, :, :3]) plt.show() return rgbImg[:, :, :3]
def getlocalMapObservation(self): # raycast around the area of the agent """ For now this includes the agent in the center of the computation """ com_p, com_o = pybullet.getBasePositionAndOrientation(self._demon) rot_matrix = pybullet.getMatrixFromQuaternion(com_o) rot_matrix = np.array(rot_matrix).reshape(3, 3) # Initial vectors # init_camera_vector = (0, 0, -1) # z-axis # init_up_vector = (0, 1, 0) # y-axis # Rotated vectors # camera_vector = rot_matrix.dot(init_camera_vector) # up_vector = rot_matrix.dot(init_up_vector) # view_matrix = p.computeViewMatrix(com_p, com_p + 0.1 * camera_vector, up_vector) if (self._fixed_view): x = 0.0 y = 0 else: x = com_p[0] y = com_p[1] view_matrix = pybullet.computeViewMatrix( cameraEyePosition=[x, y, self._observation_height], cameraTargetPosition=[x, y, 0], cameraUpVector=[0, 1, 0]) projection_matrix = pybullet.computeProjectionMatrixFOV( self._obs_fov, self._obs_aspect, self._obs_nearplane, self._obs_farplane) # img = pybullet.getCameraImage(1000, 1000, view_matrix) (w, y, img, depth, segment) = pybullet.getCameraImage(width=self._observation_shape[0], height=self._observation_shape[1], viewMatrix=view_matrix, projectionMatrix=projection_matrix) # print (img) # Don't want alpha channel img = img[..., :3] if (self._grayscale): ### Convert to Gray scale img = np.mean(img, axis=-1) return img
def render(self, width=720, height=720, far=100.0, near=0.01): # Just camera rendering function dis = 1000.0 # self.state_robot = p.getLinkState(self.pandaUid, 11)[0] # the position of end-effector # self.orien_robot = p.getLinkState(self.pandaUid, 11)[1] offset_pos = [0, 0, 0.035] target_vec = [-1, 0, 0] offset_orn = p.getQuaternionFromEuler([0, 0, 0]) view_pos, view_orn = p.multiplyTransforms(self.get_link_position(), self.get_link_orien(), offset_pos, offset_orn) target_pos, _ = p.multiplyTransforms(view_pos, view_orn, offset_pos, offset_orn) target_vector, _ = p.multiplyTransforms(view_pos, view_orn, target_vec, offset_orn) view_matrix = p.computeViewMatrix(cameraEyePosition=view_pos, cameraTargetPosition=target_pos, cameraUpVector=target_vector) proj_matrix = p.computeProjectionMatrixFOV(fov=80, aspect=float(width) / height, nearVal=near, farVal=far) (_, _, px, self.dp, _) = p.getCameraImage(width=width, height=height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px, dtype=np.uint8) rgb_array = np.reshape(rgb_array, (width, height, 4)) rgb_array = rgb_array[:, :, :3] return rgb_array, self.dp
def makeCamera(self, node, tr, parentId): self.hasCamera = True imageSize = [int(node.width), int(node.height)] viewMatrix = p.computeViewMatrix( cameraEyePosition=[tr.x(), tr.y(), tr.z()], cameraTargetPosition=[tr.x(), tr.y() + 10, tr.z()], cameraUpVector=[0, 0, 1]) eyePos = [tr.x(), tr.y() - 0.6, tr.z()] targetPos = [tr.x(), tr.y() - 5, tr.z()] upVector = [0, 0, 1] projectionMatrix = p.computeProjectionMatrixFOV(fov=node.focal, aspect=node.width / node.height, nearVal=0.3, farVal=5.1) camera = BodyCamera(viewMatrix, projectionMatrix, eyePos, targetPos, upVector, imageSize) camera.imCameraNode = node self.cameras.append(camera)
def get_camera_image(self): width, height = 128, 128 # width, height = 8, 8 # doesn't actually make much difference view = pb.computeViewMatrix( cameraEyePosition=(0, -.02, .02), cameraTargetPosition=(0, -.4, .02), # focal point cameraUpVector=(0, 0, .5), ) proj = pb.computeProjectionMatrixFOV( fov=135, aspect=height / width, nearVal=0.01, farVal=.4, ) # rgba shape is (height, width, 4) _, _, rgba, _, _ = pb.getCameraImage( width, height, view, proj, flags=pb.ER_NO_SEGMENTATION_MASK) # not much speed difference # rgba = np.empty((height, width, 4)) # much faster than pb.getCameraImage return rgba, view, proj
def takepicture(Pos, Orn): # Pos is the position of end effect, orn is the orientation of the end effect rotmatrix = p.getMatrixFromQuaternion(Orn) # distance from camera to focus distance = 0.2 # where does camera aim at camview = list() for i in range(3): camview.append(np.dot(rotmatrix[i * 3:i * 3 + 3], (0, 0, distance))) # camview[2] = camview[2] tagPos = np.add(camview, Pos) # p.removeBody(kukaId) viewMatrix = p.computeViewMatrix(Pos, tagPos, (0, 0, 1)) # viewMatrix=p.computeViewMatrix([-2, 0, 2], [0, 0, 1],[0, 0, 1]) projectMatrix = p.computeProjectionMatrixFOV( 60, 1, 0.1, 100) # input: field of view, ratio(width/height),near,far rgbpix = p.getCameraImage(128, 128, viewMatrix, projectMatrix)[2] return rgbpix[:, :, 0:3]
def get_camera_images(self): eyeid = pyplan.link_from_name(self.id, 'torso_camera_link') fov, aspect, nearplane, farplane = 80, 1.0, 0.01, 100 projection_matrix = p.computeProjectionMatrixFOV( fov, aspect, nearplane, farplane) com_p, com_o, _, _, _, _ = p.getLinkState(self.id, eyeid) rot_matrix = p.getMatrixFromQuaternion( com_o) #p.getQuaternionFromEuler((1.5707,0.866,0))) rot_matrix = np.array(rot_matrix).reshape(3, 3) # Initial vectors init_camera_vector = (1, 0, 0) # z-axis init_up_vector = (0, 1, 0) # y-axis # Rotated vectors camera_vector = rot_matrix.dot(init_camera_vector) up_vector = rot_matrix.dot(init_up_vector) view_matrix = p.computeViewMatrix(com_p + 0.25 * camera_vector, com_p + 100 * camera_vector, up_vector) imgs = p.getCameraImage(640, 640, view_matrix, projection_matrix) return imgs
def getImage(self,flag=False): position = (0, -3, 2) targetPosition = (0, 0, 2) viewMatrix = p.computeViewMatrix( position, targetPosition, cameraUpVector=[0, 0, 1], physicsClientId=self.clientId) projectionMatrix = p.computeProjectionMatrixFOV(60, 1, 0.1, 3.5,physicsClientId=self.clientId) img = p.getCameraImage(self.res[0], self.res[1], viewMatrix, projectionMatrix, renderer=p.ER_BULLET_HARDWARE_OPENGL, physicsClientId=self.clientId) img = np.reshape(img[2], (self.res[0],self.res[1], 4)) if flag: img = img[:,:,:3] else: img = img[:,:,:3] img= cv2.cvtColor(img,cv2.COLOR_RGB2GRAY) _,img = cv2.threshold(img,180,255,cv2.THRESH_BINARY_INV) return img.astype('uint8')
def get_view_matrix_from_pose(self, pose_mtx=None): '''Convert from pose matrix to view matrix suitable for pybullet render Parameters ---------- pose_mtx : array_like, optional 4x4 pose matrix that describes location of camera in world frame. If not provided, the default camera pose (from nuro_arm.constants) will be used ''' if pose_mtx is None: print('[WARNING:] no pose matrix specified for simulated camera. ' 'Camera will be placed in default location.') pose_mtx = constants.DEFAULT_CAM_POSE_MTX vecs = np.array(((0, 0, 0), (0, 0, 1), (0, -1, 0))) eye_pos, target_pos, up_vec = tfm.apply_transformation(pose_mtx, vecs) view_mtx = pb.computeViewMatrix(eye_pos, target_pos, up_vec) return view_mtx
def getCameraImage(cam_pos, cam_orn): fov = 60 aspect = 640 / 480 near = 0.01 far = 1000 angle = 0.0 q = p.getQuaternionFromEuler(cam_orn) cam_orn = np.reshape(p.getMatrixFromQuaternion(q), (3, 3)) view_pos = np.matmul(cam_orn, np.array([-0.001, 0, 0.0]).T) view_pos = np.array(view_pos + cam_pos) view_matrix = p.computeViewMatrix([cam_pos[0], cam_pos[1], cam_pos[2]], view_pos, [0, 0, 1]) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) images = p.getCameraImage(640, 480, view_matrix, projection_matrix, shadow=False, renderer=p.ER_BULLET_HARDWARE_OPENGL) return images
def main(args): workspace = np.asarray([[0.35, 0.65], [-0.15, 0.15], [0, 0.50]]) env_config = { "workspace": workspace, "max_steps": 10, "obs_size": 90, "fast_mode": False, "render": True, "goal_string": args.goal, "gen_blocks": 4, "gen_bricks": 2, "gen_triangles": 1, "gen_roofs": 1, "small_random_orientation": False, "seed": np.random.randint(100), "action_sequence": "xyrp", "simulate_grasp": True } env = createHouseBuildingXDeconstructEnv(PyBulletEnv, env_config)() env.reset() viewMatrix = pb.computeViewMatrix( cameraEyePosition=[1, 0, 0.2], cameraTargetPosition=[0, 0, 0], cameraUpVector=[0, 0, 1]) projectionMatrix = pb.computeProjectionMatrixFOV( fov=45.0, aspect=1.0, nearVal=0.1, farVal=3.1) width, height, rgbImg, depthImg, segImg = pb.getCameraImage( width=224, height=224, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix) imsave("{:s}.png".format(args.goal), rgbImg)
def _reset(self): """Called by gym.env.reset()""" ## TODO(hzyjerry): return noisy_observation # self._noisy_observation() sensor_state = BaseRobotEnv._reset(self) self.temp_target_x = self.robot.walk_target_x self.temp_target_y = self.robot.walk_target_y ## This is important to ensure potential doesn't change drastically when reset self.potential = self.robot.calc_potential() staticMat = p.computeViewMatrix([0, 0, 0], [1, 1, 1], [0, 0, 1]) static_cam = { 'yaw': -130, 'pitch': -30, 'distance': 9.9, 'target': [-1.253, -4.94, 1.05] } #p.resetDebugVisualizerCamera(static_cam['distance'], static_cam['yaw'], static_cam['pitch'], static_cam['target']) #staticImg = p.getCameraImage(self.windowsz, self.windowsz)[2] if not self._require_camera_input or self.test_env: visuals = self.get_blank_visuals() return visuals, sensor_state eye_pos, eye_quat = self.get_eye_pos_orientation() pose = [eye_pos, eye_quat] all_dist, all_pos = self.r_camera_rgb.rankPosesByDistance(pose) top_k = self.find_best_k_views(eye_pos, all_dist, all_pos) self.render_rgb, self.render_depth, self.render_semantics, self.render_normal, self.render_unfilled = self.r_camera_rgb.renderOffScreen( pose, top_k) visuals = self.get_visuals(self.render_rgb, self.render_depth) target_x = self.temp_target_x target_y = self.temp_target_y #p.resetBasePositionAndOrientation(self.target_mapId, [target_x / self.robot.mjcf_scaling, target_y / self.robot.mjcf_scaling, 6], [0, 0, 0, 1]) return visuals, sensor_state
def get_image(robotID, width=200, height=200): cam_link_state = p.getLinkState(robotID, linkIndex=7, computeForwardKinematics=1) pos = cam_link_state[-2] ort = cam_link_state[-1] rot_mat = np.array(p.getMatrixFromQuaternion(ort)) rot_mat = np.reshape(rot_mat, [3, 3]) dir = rot_mat.dot([1, 0, 0]) up_vector = rot_mat.dot([0, 0, 1]) s = 0.01 view_matrix = p.computeViewMatrix(pos, pos + s * dir, up_vector) # p.addUserDebugText(text=".",textPosition=pos+s*dir,textColorRGB=[1,0,0],textSize=10) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) # f_len = projection_matrix[0] ld = [ random.uniform(-20, 20), random.uniform(-20, 20), random.uniform(10, 20) ] lc = [random.random(), random.random(), random.random()] if random.random() > 0.0: _, _, rgbImg, depthImg_buffer, segImg = p.getCameraImage( width, height, view_matrix, projection_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) else: _, _, rgbImg, depthImg_buffer, segImg = p.getCameraImage( width, height, view_matrix, projection_matrix, renderer=p.ER_TINY_RENDERER, lightColor=lc, lightAmbientCoeff=random.random(), lightDiffuseCoeff=random.random(), lightSpecularCoeff=random.random()) A = np.reshape(rgbImg, (width, height, 4))[:, :, :3] return Image.fromarray(A), depthImg_buffer, segImg
def __init__(self, width=400, height=300, fov=60, pos=[.5, .5, .5], look_at=[0, 0, 0]): self.width = width self.heigt = height self.view = p.computeViewMatrix(cameraEyePosition=pos, cameraTargetPosition=look_at, cameraUpVector=[0, 0, 1]) self.proj = p.computeProjectionMatrixFOV(fov=fov, aspect=width / height, nearVal=0.1, farVal=0.8) # if this should ever trigger, the official PyPi version (pip install pybullet) is compiled with Numpy headers assert p.isNumpyEnabled( ) # because if not, copying the pixels of the camera image from C++ to python takes forever
def __init__(self, size=None, cameraEyePosition=[0, 0, 0.5], fov=60, cameraTargetPosition=[0, 0.5, 0.5]): if size is None: size = Camera.default_size self.size = size self.viewMatrix = pb.computeViewMatrix( cameraEyePosition=cameraEyePosition, cameraTargetPosition=cameraTargetPosition, cameraUpVector=[0, 0, 1]) self.projectionMatrix = pb.computeProjectionMatrixFOV(fov=fov, aspect=1.0, nearVal=0.1, farVal=100) self.cam_image_kwargs = { **self.size, 'viewMatrix': self.viewMatrix, 'projectionMatrix': self.projectionMatrix, 'renderer': pb.ER_TINY_RENDERER }
def get_camera_image(robot, link_id, projection_matrix): link_info = p.getLinkState(robot, link_id['head']) #Get head link info link_pos = link_info[0] #Get link com pos wrt world link_ori = link_info[1] #Get link com ori wrt world rot = p.getMatrixFromQuaternion(link_ori) rot = np.array(rot).reshape(3, 3) global_camera_x_unit = np.array([1, 0, 0]) global_camera_z_unit = np.array([0, 0, 1]) camera_eye_pos = link_pos + np.dot(rot, 0.1 * global_camera_x_unit) camera_target_pos = link_pos + np.dot(rot, 1.0 * global_camera_x_unit) camera_up_vector = np.dot(rot, global_camera_z_unit) view_matrix = p.computeViewMatrix(camera_eye_pos, camera_target_pos, camera_up_vector) width, height, rgb_img, depth_img, seg_img = p.getCameraImage( 50, #image width 10, #image height view_matrix, projection_matrix) return width, height, rgb_img, depth_img, seg_img
def capture_image(self): # Camera *_, camera_position, camera_orientation = p.getLinkState( self.robot, self.camera_link) camera_look_position, _ = p.multiplyTransforms(camera_position, camera_orientation, [0, 0.1, 0], [0, 0, 0, 1]) view_matrix = p.computeViewMatrix( cameraEyePosition=camera_position, cameraTargetPosition=camera_look_position, cameraUpVector=(0, 0, 1)) projection_matrix = p.computeProjectionMatrixFOV(fov=45.0, aspect=1.0, nearVal=0.1, farVal=3.1) return p.getCameraImage(300, 300, view_matrix, projection_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]
def setCameraPicAndGetPic(robot_id: int, width: int = 224, height: int = 224, physicsClientId: int = 0): """ 给合成摄像头设置图像并返回robot_id对应的图像 摄像头的位置为miniBox前头的位置 """ basePos, baseOrientation = p.getBasePositionAndOrientation( robot_id, physicsClientId=physicsClientId) # 从四元数中获取变换矩阵,从中获知指向(左乘(1,0,0),因为在原本的坐标系内,摄像机的朝向为(1,0,0)) matrix = p.getMatrixFromQuaternion(baseOrientation, physicsClientId=physicsClientId) tx_vec = np.array([matrix[0], matrix[3], matrix[6]]) # 变换后的x轴 tz_vec = np.array([matrix[2], matrix[5], matrix[8]]) # 变换后的z轴 basePos = np.array(basePos) # 摄像头的位置 cameraPos = basePos + BASE_RADIUS * tx_vec + 0.5 * BASE_THICKNESS * tz_vec targetPos = cameraPos + 1 * tx_vec viewMatrix = p.computeViewMatrix(cameraEyePosition=cameraPos, cameraTargetPosition=targetPos, cameraUpVector=tz_vec, physicsClientId=physicsClientId) projectionMatrix = p.computeProjectionMatrixFOV( fov=50.0, # 摄像头的视线夹角 aspect=1.0, nearVal=0.01, # 摄像头焦距下限 farVal=20, # 摄像头能看上限 physicsClientId=physicsClientId) width, height, rgbImg, depthImg, segImg = p.getCameraImage( width=width, height=height, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, physicsClientId=physicsClientId) return width, height, rgbImg, depthImg, segImg
def main_usingEnvOnly(): # environment = KukaGymEnv(renders=True,isDiscrete=False, maxSteps = 10000000) environment = KukaCamGymEnv_Reconfigured(renders=True,isDiscrete=False) environment._reset() #p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) randomObjs = add_random_objs_to_scene(10) motorsIds=[] motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537)) motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0)) motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2)) motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) # dv = 0.01 # motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) # motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) # motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0)) # motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) # motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) done = False #According to the hand-eye coordination paper, the camera is mounted over the shoulder of the arm camEyePos = [0.03,0.236,0.54] targetPos = [0.640000,0.075000,-0.190000] cameraUp = [0,0,1] viewMat = p.computeViewMatrix(camEyePos,targetPos,cameraUp) camInfo = p.getDebugVisualizerCamera() projMatrix = camInfo[3] # viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] # projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] img_arr = p.getCameraImage(640,512,viewMatrix=viewMat,projectionMatrix=projMatrix)#640*512*3 # write_from_imgarr(img_arr, 1) while (not done): action=[] for motorId in motorsIds: action.append(environment._p.readUserDebugParameter(motorId)) state, reward, done, info = environment.step2(action) obs = environment.getExtendedObservation()
def __init__(self, urdfRootPath='', pos=[-0.2, 0.0, -.12], orn=[0, 0, 0, 1.0], render=True, global_t=0): self.lf_id = 8 self.rf_id = 9 self.gripper_close_pose = 0.06 self.maxForce = 1000 #200. self.scale = 0.01 self.object_gripper_offset = 0 self.fingerTipForce = 8 self.is_gripper_open = True self.kukaId = p.loadSDF( os.path.join(urdfRootPath, "kuka_iiwa/kuka_with_gripper5.sdf"), globalScaling=1)[0] self.kukaEndEffectorIndex = 7 #Link index of gripper base p.resetBasePositionAndOrientation(self.kukaId, pos, orn) self.numJoints = p.getNumJoints(self.kukaId) far = 1 #near = 0.001 #self.cam_viewMatrix = list(p.computeViewMatrix(cameraEyePosition=[0, 0, 0.5], # cameraTargetPosition=[0.4, 0, 0.2], cameraUpVector = [0.0, 0.0, 1.0])) #self.cam_projMatrix = p.computeProjectionMatrixFOV(80, 1, near, far); #self.img_size_h = 200#700 near = 0.1 self.cam_viewMatrix = list( p.computeViewMatrix( cameraEyePosition=[-0.2, 0, 0.04], cameraTargetPosition=[0.4, 0, -0.1], cameraUpVector=[0.6, 0.0, 0.8])) self.cam_projMatrix = p.computeProjectionMatrixFOV(100, 1, near, far) self.img_size_h = 360 #700 self.img_size_w = 360 #1280 self.far = far self.near = near self.render = render self.global_t = global_t #for debugging self.default = {}
def render(self, mode='rgb_array', close=False): if mode != "rgb_array": return np.array([]) base_pos, orn = p.getBasePositionAndOrientation(self.car) yaw = p.getEulerFromQuaternion(orn)[2] # z軸方向から見た本体の回転角度 rot_matrix = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) # 回転行列 target_relative_vec2D = np.array([2, 0]) # 本体から見たtargetの相対位置 target_abs_vec2D = np.dot(rot_matrix, target_relative_vec2D) # targetの絶対位置 cam_eye = np.array(base_pos) + np.array([0, 0, 0.2]) cam_target = np.array(base_pos) + np.append(target_abs_vec2D, 0.2) # z=0.2は足() cam_upvec = [0, 0, 1] view_matrix = p.computeViewMatrix(cameraEyePosition=cam_eye, cameraTargetPosition=cam_target, cameraUpVector=cam_upvec) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(self.width) / self.height, nearVal=0.1, farVal=100.0) (_, _, rgb, _, mask) = p.getCameraImage(width=self.width, height=self.height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(rgb) rgb_array = rgb_array[:, :, :3] mask_array = np.array(mask) return rgb_array
def getlocalMapObservation(self): # raycast around the area of the agent """ For now this includes the agent in the center of the computation """ com_p, com_o = pybullet.getBasePositionAndOrientation(self._demon) rot_matrix = pybullet.getMatrixFromQuaternion(com_o) rot_matrix = np.array(rot_matrix).reshape(3, 3) # Initial vectors # init_camera_vector = (0, 0, -1) # z-axis # init_up_vector = (0, 1, 0) # y-axis # Rotated vectors # camera_vector = rot_matrix.dot(init_camera_vector) # up_vector = rot_matrix.dot(init_up_vector) # view_matrix = p.computeViewMatrix(com_p, com_p + 0.1 * camera_vector, up_vector) # Make the observation track the agent. view_matrix = pybullet.computeViewMatrix(cameraEyePosition=[com_p[0], com_p[1], self._observation_height], cameraTargetPosition=[com_p[0], com_p[1], 0], cameraUpVector=[0, 1, 0]) # This will make it look similar to the rendering. # view_matrix = pybullet.computeViewMatrix( # cameraEyePosition=[0, 0, 15], # cameraTargetPosition=[0, 0, 0], # cameraUpVector=[0, 1, 0]) projection_matrix = pybullet.computeProjectionMatrixFOV( self._obs_fov, self._obs_aspect, self._obs_nearplane, self._obs_farplane) # img = pybullet.getCameraImage(1000, 1000, view_matrix) (w,y,img,depth,segment) = pybullet.getCameraImage( width=self._observation_shape[0], height=self._observation_shape[1], viewMatrix=view_matrix, projectionMatrix=projection_matrix) # print (img) # Don't want alpha channel img = img[..., :3] return img
def get_image(self, imsize=(214, 214), plot_cam_pose=False): cam_trans, cam_rot = self.get_cam_pose() cam_rotm = np.array(p.getMatrixFromQuaternion(cam_rot)).reshape(3, 3) if plot_cam_pose: draw_pose(cam_trans, cam_rot) # Calculate extrinsic matrix # target position is camera frame y axis tip in global frame # up vector is the z axis of camera frame viewMatrix = p.computeViewMatrix( cameraEyePosition=cam_trans, cameraTargetPosition=(cam_rotm.dot(np.array([0, 1, 0]).T) + np.array(cam_trans)).tolist(), cameraUpVector=cam_rotm[:, 2].tolist()) width, height, rgbImg, depthImg, segImg = p.getCameraImage( width=imsize[0], height=imsize[1], viewMatrix=viewMatrix, projectionMatrix=self.projectionMatrix, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) # depth image transformed to true depth, each pixel in segImg is the linkID return rgbImg[:, :, :3], get_true_depth( depthImg, self.z_near, self.z_far), ((segImg >> 24) - 1)
def _get_onrobot_observation(self): """Return the observation as an image. """ camerapos = p.getLinkState(self._tm700.tm700Uid, self._tm700.tmEndEffectorIndex + 2) blockPos, blockOrn = p.getBasePositionAndOrientation( self._objectUids[0]) camerapos = camerapos[0] print(camerapos) distance = 1 # camerapos[2] pitch = -90 #-60 + self._cameraRandom * np.random.uniform(-3, 3) #original -56 yaw = 0 # 245 + self._cameraRandom * np.random.uniform(-3, 3) roll = 0 # self._view_matrix = p.computeViewMatrixFromYawPitchRoll(camerapos, distance, yaw, pitch, roll, 2) self._view_matrix = p.computeViewMatrix(cameraEyePosition=camerapos, cameraTargetPosition=blockPos, cameraUpVector=[0, 0, 1]) img_arr = p.getCameraImage(width=self._width, height=self._height, viewMatrix=self._view_matrix, projectionMatrix=self._proj_matrix) rgb = img_arr[2] depth = img_arr[3] min = 0.97 max = 1.0 print(depth) depthnormalized = (float(depth) - min) / (max - min) segmentation = img_arr[4] depth = np.reshape(depth, (self._height, self._width, 1)) segmentation = np.reshape(segmentation, (self._height, self._width, 1)) np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) np_img_arr = np_img_arr.astype(np.float64) test = np.concatenate([np_img_arr[:, :, 1:3], depth], axis=-1) return test
def _take_picture(self): # get the last link's position and orientation Pos, Orn = p.getLinkState(self.robot_id, self.robot_joint_num - 1)[:2] # Pos is the position of end effect, orn is the orientation of the end effect rotmatrix = p.getMatrixFromQuaternion(Orn) # distance from camera to focus distance = 0.2 # where does camera aim at camview = list() for i in range(3): camview.append(np.dot(rotmatrix[i * 3:i * 3 + 3], (0, 0, distance))) tagPos = np.add(camview, Pos) #p.removeBody(kukaId) viewMatrix = p.computeViewMatrix(Pos, tagPos, (0, 0, 1)) viewMatrix = [round(elem, 2) for elem in viewMatrix] projectMatrix = p.computeProjectionMatrixFOV( 60, 1, 0.1, 100) # input: field of view, ratio(width/height),near,far projectMatrix = [round(elem, 2) for elem in projectMatrix] rgbpix = p.getCameraImage(XSIZE, YSIZE, viewMatrix, projectMatrix)[2] return rgbpix[:, :, 0:3]
def _get_seg_mask_for_target(target_position, cam_position, world=None): """ Calculates the view and projection Matrix and returns the Segmentation mask The segmentation mask indicates for every pixel the visible Object. :param cam_position: The position of the Camera as a list of x,y,z and orientation as quaternion :param target_position: The position to which the camera should point as a list of x,y,z and orientation as quaternion :return: The Segmentation mask from the camera position """ world, world_id = _world_and_id(world) fov = 300 aspect = 256 / 256 near = 0.2 far = 10 view_matrix = p.computeViewMatrix(cam_position[0], target_position[0], [0, 0, -1]) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) return p.getCameraImage(256, 256, view_matrix, projection_matrix, physicsClientId=world_id)[4]
def init(useGUI): if useGUI: p.connect(p.GUI) else: p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 0) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) p.setAdditionalSearchPath(pdata.getDataPath()) p.setGravity(0, 0, -9.81) p.setRealTimeSimulation(0) # Compute the view and image from camera view_matrix = p.computeViewMatrix([0.575, 0, 0.5], [0.575, 0, 0], [-1, 0, 0]) projection_matrix = p.computeProjectionMatrixFOV( fov=45.0, aspect=1, nearVal=0.1, farVal=0.55) return view_matrix, projection_matrix
p.loadURDF('plane.urdf') p.loadURDF("r2d2.urdf",[0,0,1]) p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025]) cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025]) p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1]) width = 128 height = 128 fov = 60 aspect = width / height near = 0.02 far = 1 view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0]) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) # Get depth values using the OpenGL renderer images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255. depth_buffer_opengl = np.reshape(images[3], [width, height]) depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl) seg_opengl = np.reshape(images[4], [width, height])*1./255. time.sleep(1) # Get depth values using Tiny renderer images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER) depth_buffer_tiny = np.reshape(images[3], [width, height]) depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny) rgb_tiny= np.reshape(images[2], (height, width, 4))*1./255.