def render(self, mode="human"): view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[0.55, 0.35, 0.2], distance=1.2, yaw=0, pitch=-70, roll=0, upAxisIndex=2, ) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(960) / 720, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage( width=960, height=720, 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, (720, 960, 4)) rgb_array = rgb_array[:, :, :3] if is_notebook() and os.path.exists("/tmp/render.txt"): display(rgb_data) return rgb_array
def camera_feed(self, is_flat = False): """ Function to get camera feed of the arena. Arguments: None Return Values: numpy array of RGB values """ look = [0, 0, 0.2] cameraeyepos = [0, 0, 6.5] cameraup = [0, -1, 0] self._view_matrix = p.computeViewMatrix(cameraeyepos, look, cameraup) fov = 75 aspect = self._width / self._height near = 0.8 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) img_arr = p.getCameraImage(width=self._width, height=self._height, viewMatrix=self._view_matrix, projectionMatrix=self._proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb = img_arr[2] if is_flat == True: # Only for those who are getting a blank image in opencv rgb = np.array(rgb) rgb = np.reshape(rgb, (512, 512, 4)) rgb = np.uint8(rgb) rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB) rgb = np.rot90(rgb, 3) return rgb
def follow_agent(agent: Agent, width=640, height=480) -> np.ndarray: position, orientation = pybullet.getBasePositionAndOrientation( agent.vehicle_id) _, _, yaw = pybullet.getEulerFromQuaternion(orientation) orientation = pybullet.getQuaternionFromEuler((0, 0, yaw)) rot_matrix = pybullet.getMatrixFromQuaternion(orientation) rot_matrix = np.array(rot_matrix).reshape(3, 3) camera_position = position + rot_matrix.dot([-0.8, 0, 0.3]) up_vector = rot_matrix.dot([0, 0, 1]) target = position view_matrix = pybullet.computeViewMatrix(camera_position, target, up_vector) proj_matrix = pybullet.computeProjectionMatrixFOV(fov=60, aspect=float(width) / height, nearVal=0.01, farVal=10.0) _, _, rgb_image, _, _ = pybullet.getCameraImage( width=width, height=height, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, viewMatrix=view_matrix, projectionMatrix=proj_matrix) rgb_array = np.reshape(rgb_image, (height, width, -1)) rgb_array = rgb_array[:, :, :3] return rgb_array
def getExtendedObservation(p, vehicle_id): carpos, carorn = p.getBasePositionAndOrientation(vehicle_id) # Using the ZED calculations, I needed to add in rotation too. rotate = p.getQuaternionFromEuler([0, 0, np.pi]) carpos, carorn = p.multiplyTransforms(carpos, carorn, [0, 0, .1], rotate) # Most of this is cribbed from racecarZEDGymEnv.py carmat = p.getMatrixFromQuaternion(carorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0], carpos[1] + dist0*carmat[3], carpos[2]+dist0*carmat[6]] targetPos = [carpos[0]+dist1*carmat[0], carpos[1] + dist1*carmat[3], carpos[2]+dist1*carmat[6]] up = [carmat[2], carmat[5], carmat[8]] # These things taken mostly from testrender_np.py pixelWidth = 64 pixelHeight = 64 nearPlane = 0.01 farPlane = 100 fov = 60 viewMatrix = p.computeViewMatrix(eyePos, targetPos, up) aspect = pixelWidth / pixelHeight projectionMatrix = p.computeProjectionMatrixFOV( fov, aspect, nearPlane, farPlane) img_arr = p.getCameraImage( pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=1, lightDirection=[1, 1, 1], renderer=p.ER_BULLET_HARDWARE_OPENGL) return img_arr
def __init__(self, pose_mtx=None, pb_client=0): '''Class to allow asynchronous video capture within pybullet simulator Parameters ---------- view_mtx : array_like, optional 4x4 view matrix that describes location of camera in world frame. If not provided, the default camera pose (from nuro_arm.constants) will be used pb_client : int, default to 0 Physics Client ID describing which simulator the camera exists within. If you are only using one simulator, you can leave as default. ''' super().__init__() # create projection matrix self._img_width, self._img_height = constants.NEW_CAM_ROI[2:] fov = np.degrees( 2 * np.arctan2(self._img_height, 2 * constants.NEW_CAM_MTX[1, 1])) aspect = self._img_width / self._img_height self._proj_mtx = pb.computeProjectionMatrixFOV(fov, aspect, 0.001, 10) # create view matrix self._view_mtx = self.get_view_matrix_from_pose(pose_mtx) self._pb_client = pb_client
def render_map(self): base_pos = [0, 0, -3] if (hasattr(self, 'robot')): if (hasattr(self.robot, 'body_xyz')): base_pos[0] = self.robot.body_xyz[0] base_pos[1] = self.robot.body_xyz[1] view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=35, yaw=0, pitch=-89, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array
def set_camera(self): # TODO: アーム先端部のworld座標 rot_mtrx = np.array( p.getMatrixFromQuaternion(self.link_info['ee_link'][1])).reshape( 3, 3) self.view_matrix = p.computeViewMatrix( cameraEyePosition=np.array(self.link_info['ee_link'][0]), cameraTargetPosition=np.array(self.link_info['ee_link'][0]) + rot_mtrx @ np.array([1, 0, 0]), cameraUpVector=rot_mtrx @ np.array([0, 1, 0])) # self.view_matrix = p.computeViewMatrixFromYawPitchRoll( # cameraTargetPosition=np.array([0.5, -0.5, 1.5]) * 1/1,#np.array([4, -4, 3]) * 1/10, # target focus point # distance=2., # from eye to target # yaw=30, # degree left/right around z axis # pitch=-30, # degree up/down around # roll=90, # degree around forward vector # upAxisIndex=2) # either 1 for Y or 2 for Z axis up # 表示のための投影マトリクス計算 self.proj_matrix = p.computeProjectionMatrixFOV( fov=60, # the field of view angle, in degrees, in the y direction aspect=320 / 240, # the aspect ratio that determines the field of view in the x direction. The aspect ratio is the ratio of x (width) to y (height). nearVal= 0.1, # the distance from the viewer to the near clipping plane (always positive) farVal=100.0 ) # the distance from the viewer to the far clipping plane (always positive)
def render(self, mode='human'): base_pos = [2, 1.4, 3.5] view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=1.0, yaw=0, pitch=-80, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(480) / 480, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=480, height=480, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] if mode == "rgb_array": return rgb_array elif mode == 'human': if self.viewer is None: self.viewer = rendering.SimpleImageViewer() self.viewer.imshow(rgb_array) return self.viewer.isopen
def run_example(): times = [] dim = 5 env = Shapes3D(gui=0, env_dim=dim) env.reset() env.add_sphere(0.2, [1, 1, 1, 1], [1, 1, 0.2]) env.add_cube([0.2, 0.2, 0.2], [0, 0, 1, 1], [-1, 1, 0.2]) env.add_capsule(0.2, 1, [1, 0, 0, 1], [-1, -1, 0.7]) env.add_cylinder(0.4, 1.5, [1, 0, 0, 1], [1, -1, 0.75]) intrinsic = p.computeProjectionMatrixFOV(fov=45, aspect=1, nearVal=.1, farVal=dim * 2) for i in itertools.count(0, 1): start = time.time() extrinsic = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[1, 1, 1], distance=2.5, yaw=i * 5, pitch=-17, roll=0, upAxisIndex=2) env.compute_render(320, 320, intrinsic, extrinsic) end = time.time() print("rendering took: %f" % (end - start)) times.append(end - start) if i > 1000: break env.reset() # Not needed, testing purposes only env.destroy() print("avg of %f per render" % (sum(times) / 1000))
def __init__(self, height, width, window_name, fov=60): camPos = [0, 0, 0] camUpVector = [0, -1, 0] camTargetPos = [0, 0, 1] nearPlane = 0.01 farPlane = 100 self.window_name = window_name self.height = height self.width = width ############## VTK stuff ############## # Create a rendering window and renderer self.ren = vtk.vtkRenderer() self.renWin = vtk.vtkRenderWindow() self.renWin.SetOffScreenRendering(1) self.renWin.SetAlphaBitPlanes(1) # Enable usage of alpha channel self.renWin.AddRenderer(self.ren) self.renWin.SetSize(width, height) self.camera = self.ren.GetActiveCamera() self.camera.SetPosition(camPos) self.camera.SetViewUp(camUpVector) self.camera.SetFocalPoint(camTargetPos) self.camera.SetViewAngle(fov) self.w2if = vtk.vtkWindowToImageFilter() self.w2if.SetInput(self.renWin) self.w2if.SetInputBufferTypeToRGBA() ############## pybullet stuff ############## p.connect(p.DIRECT) # conid = p.connect(p.SHARED_MEMORY) # if (conid<0): # p.connect(p.GUI) p.resetSimulation() p.setGravity(0, 0, -10) # p.setPhysicsEngineParameter(numSolverIterations=5) # p.setPhysicsEngineParameter(fixedTimeStep=1./240.) # p.setPhysicsEngineParameter(numSubSteps=10) self.viewMatrix = p.computeViewMatrix(camPos, camTargetPos, camUpVector) aspect = width / height self.projectionMatrix = p.computeProjectionMatrixFOV( fov, aspect, nearPlane, farPlane) egl = pkgutil.get_loader('eglRenderer') if (egl): p.loadPlugin(egl.get_filename(), "_eglRendererPlugin") else: p.loadPlugin("eglRendererPlugin") self.play = True self.physics_thread = Thread(target=self.update_physics, args=()) self.physics_thread.daemon = True print("READY")
def render(self): # the target position in camera view will be in half meter [0.5, 0, 0] away from robot base [0,0,0] # the distance of 1.5m away from top of table makes sense for the robot which in longest length is about 1m. view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[-0.5, 0, 0], distance=1.5, yaw=90, pitch=-90, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(960) / 720, nearVal=0.1, farVal=100.0) (width, height, rgbPixels, depthPixels, SegMaskBuffer) = p.getCameraImage( width=960, height=720, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(rgbPixels, dtype=np.uint8) rgb_array = np.reshape(rgb_array, (height, width, 4)) # the default internal values are (1; enabled); if 0 will be disabled. p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 1) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 1) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 1) # return a tuple for rgb data and depth values return (rgb_array, depthPixels)
def render(self, mode='rgb_array', close=False): if mode != "rgb_array": return np.array([]) base_pos, orn = p.getBasePositionAndOrientation(self.car) cam_eye = np.array(base_pos) + [0.1,0,0.2] cam_target = np.array(base_pos) + [2,0,0.2] cam_upvec = [1,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 getCameraImageEEF(pos, orn): #print(eef_pose) com_p = pos com_o = orn rot_matrix = p.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[0], com_p[1], com_p[2]], [0, 0, 0], up_vector) fov = 60 aspect = imageWidth / imageHeight near = 0.01 far = 1000 angle = 0.0 projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) images = p.getCameraImage(imageWidth, imageHeight, view_matrix, projection_matrix, shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL) return images, view_matrix
def render(self, mode="rgb_array", close=False): if mode != "rgb_array": return np.array([]) cam_dist = 1.3 cam_yaw = 180 cam_pitch = -40 RENDER_HEIGHT = 720 RENDER_WIDTH = 960 base_pos, orn = p.getBasePositionAndOrientation(self.robot_id) view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=cam_dist, yaw=cam_yaw, pitch=cam_pitch, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(RENDER_WIDTH) / RENDER_HEIGHT, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) #renderer=self._p.ER_TINY_RENDERER) rgb_array = np.array(px, dtype=np.uint8) rgb_array = np.reshape(rgb_array, (RENDER_HEIGHT, RENDER_WIDTH, 4)) rgb_array = rgb_array[:, :, :3] return rgb_array
def __init__(self): self.physicsClient = p.connect( p.DIRECT) # or p.DIRECT for non-graphical version startPos = [0, 0, -0.181] startOrientation = p.getQuaternionFromEuler([0, 0, 0]) self.robot = p.loadURDF( "hdt_michigan_description/urdf/left_gripper_no_housing.urdf", startPos, startOrientation, physicsClientId=self.physicsClient) # set jdict, maps from joint name to joint index self.jdict = {} for i in range( p.getNumJoints(self.robot, physicsClientId=self.physicsClient)): info = p.getJointInfo(self.robot, i, physicsClientId=self.physicsClient) jname = info[1].decode("ascii") self.jdict[jname] = i #print(self.jdict) self.joint_ids = [ self.jdict["leftgripper"], self.jdict["leftgripper2"] ] # camera stuff self.z_near = 0.1 self.z_far = 3.1 self.projectionMatrix = p.computeProjectionMatrixFOV( fov=65.0, aspect=1.0, nearVal=self.z_near, farVal=self.z_far)
def get_gripper_camera(self, imgSize): # camera_angle = [0, np.pi / 12, 0] camera_relative_loc = [-0.1, -0.15] girperLocation = p.getLinkState(self.__gripper_body_id, 0) camera_t_R = p.getMatrixFromQuaternion(p.getQuaternionFromEuler(camera_angle)) maxtrix_gri = p.getMatrixFromQuaternion(girperLocation[1]) maxtrix_cam = np.array(maxtrix_gri) * np.array(camera_t_R) camlookat = [maxtrix_cam[2] + girperLocation[0][0], maxtrix_cam[5] + girperLocation[0][1], maxtrix_cam[8] + girperLocation[0][2]] camup = [maxtrix_cam[1], maxtrix_cam[4], maxtrix_cam[7]] girpper_up = [maxtrix_gri[1], maxtrix_gri[4], maxtrix_gri[7]] gripper_fwd = [maxtrix_gri[2], maxtrix_gri[5], maxtrix_gri[8]] camera_loc = np.array(girperLocation[0]) + np.array(girpper_up) * camera_relative_loc[0] + np.array( gripper_fwd) * camera_relative_loc[1] view_matrix = p.computeViewMatrix(camera_loc.tolist(), camlookat, camup) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=1, nearVal=0.01, farVal=10.0) img_arr = p.getCameraImage(imgSize, imgSize, view_matrix, proj_matrix, shadow=1, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) return img_arr
def update(self): # Guidance # Only do this if we are not being manually controlled if not self.keyboard_control: self.ltarget_vel, self.rtarget_vel = next(self.vel_gen) # Movement p.setJointMotorControlArray(self.robot, self.motor_links, p.VELOCITY_CONTROL, targetVelocities=[ self.velocity_limit * self.rtarget_vel, self.velocity_limit * self.ltarget_vel ], forces=[self.max_force, self.max_force]) # 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)
def get_image(self, pos, forward, up=None, fov=90., aspect=4 / 3, height=720): pos = totensor(pos) forward = totensor(forward) if up is not None: up = totensor(up) else: left = torch.cross(torch.tensor([0., 0., 1.]), forward) up = torch.cross(forward, left) view_mat = p.computeViewMatrix(cameraEyePosition=pos.tolist(), cameraTargetPosition=forward.tolist(), cameraUpVector=up.tolist(), physicsClientId=self.sim.id) NEAR_PLANE = 0.01 FAR_PLANE = 1000.0 proj_mat = p.computeProjectionMatrixFOV(fov=fov, aspect=aspect, nearVal=NEAR_PLANE, farVal=FAR_PLANE, physicsClientId=self.sim.id) _, _, img, depth, mask = p.getCameraImage(width=int(aspect * height), height=height, viewMatrix=view_mat, projectionMatrix=proj_mat, physicsClientId=self.sim.id) img = torch.tensor(img) depth = FAR_PLANE * NEAR_PLANE / ( FAR_PLANE - (FAR_PLANE - NEAR_PLANE) * torch.tensor(depth)) mask = torch.tensor(mask) return {"img": img, "depth": depth, "mask": mask}
def render(self, mode='human'): view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[0.7, 0, 0.05], distance=0.7, yaw=90, pitch=-70, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(960) / 720, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=960, height=720, 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, (720, 960, 4)) rgb_array = rgb_array[:, :, :3] return rgb_array
def render(size=img_size, eye=cam_eye, target=cam_target, up=cam_up, fov=45, aspect=1.0, nearVal=0.1, farVal=30.1): viewMatrix = pb.computeViewMatrix( cameraEyePosition=eye, cameraTargetPosition=target, cameraUpVector=up, ) projectionMatrix = pb.computeProjectionMatrixFOV(fov=fov, aspect=aspect, nearVal=nearVal, farVal=farVal) w, h, rgba_img, depth_img, seg_img = pb.getCameraImage( width=size, height=size, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix) rgb_img = rgba_img[:, :, :-1] return rgb_img
def get_camera_image_from_link(robot, link, pic_width, pic_height, fov, nearval, farval): aspect = pic_width / pic_height projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearval, farval) link_info = p.getLinkState(robot, link, 1, 1) #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) #SE3_camera_to_world width, height, rgb_img, depth_img, seg_img = p.getCameraImage( pic_width, #image width pic_height, #image height view_matrix, projection_matrix) return width, height, rgb_img, depth_img, seg_img, view_matrix, projection_matrix, camera_eye_pos
def birds_eye(agent: Agent, width=640, height=480) -> np.ndarray: position, _ = pybullet.getBasePositionAndOrientation(agent.vehicle_id) position = np.array([position[0], position[1], 3.0]) view_matrix = pybullet.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=position, distance=3.0, yaw=0, pitch=-90, roll=0, upAxisIndex=2) proj_matrix = pybullet.computeProjectionMatrixFOV(fov=90, aspect=float(width) / height, nearVal=0.01, farVal=100.0) _, _, rgb_image, _, _ = pybullet.getCameraImage( width=width, height=height, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, viewMatrix=view_matrix, projectionMatrix=proj_matrix) rgb_array = np.reshape(rgb_image, (height, width, -1)) rgb_array = rgb_array[:, :, :3] return rgb_array
def render_physics(self): robot_pos, _ = p.getBasePositionAndOrientation(self.robot_tracking_id) view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=robot_pos, distance=self.tracking_camera["distance"], yaw=self.tracking_camera["yaw"], pitch=self.tracking_camera["pitch"], roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) with Profiler("render physics: Get camera image"): (_, _, px, _, _) = p.getCameraImage(width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_TINY_RENDERER) rgb_array = np.array(px).reshape( (self._render_width, self._render_height, -1)) rgb_array = rgb_array[:, :, :3] return rgb_array
def _render(self, mode, close): if (mode=="human"): self.isRender = True if mode != "rgb_array": return np.array([]) base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width)/self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage( width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array
def __init__(self): try: isInit except: isInit = True CLIENT = pybullet.connect(pybullet.GUI) print("client", CLIENT) pb.setAdditionalSearchPath( pybullet_data.getDataPath()) #used by loadURDF plane = pybullet.loadURDF("plane.urdf") print("plane ID", plane) #pb.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0, physicsClientId=CLIENT) #pb.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0, physicsClientId=CLIENT) pb.setGravity(0, 0, -10.0) self.table = pb.loadURDF("table/table.urdf") print("table ID", self.table) self.plate = pb.loadURDF("dish/plate.urdf") print("plate ID", self.plate) self.rgripper = RGripper() self.lgripper = LGripper() #self.robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.pr2_urdfpath()) self.robot_model = PR2() interface = PybulletRobotInterface(self.robot_model) self.try_num = 100 #Simulator loop times self.frames = [] #Movie buffer pb.resetDebugVisualizerCamera(2.0, 90, -0.0, (0.0, 0.0, 1.0)) # For pybullet getCameraImage argument self.viewMatrix = pb.computeViewMatrix(cameraEyePosition=[5, 5, 30], cameraTargetPosition=[3, 3, 3], cameraUpVector=[3, 1, 3]) self.projectionMatrix = pb.computeProjectionMatrixFOV(fov=45.0, aspect=1.0, nearVal=0.1, farVal=3.1)
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 render(self, mode='human'): if self.rendered_img is None: self.rendered_img = plt.imshow(np.zeros((100, 100, 4))) # 基本資訊 car_id, client_id = self.car.get_ids() proj_matrix = p.computeProjectionMatrixFOV(fov=80, aspect=1, nearVal=0.01, farVal=100) pos, ori = [ list(l) for l in p.getBasePositionAndOrientation(car_id, client_id) ] pos[2] = 0.2 # 旋轉照相機方向 rot_mat = np.array(p.getMatrixFromQuaternion(ori)).reshape(3, 3) camera_vec = np.matmul(rot_mat, [1, 0, 0]) up_vec = np.matmul(rot_mat, np.array([0, 0, 1])) view_matrix = p.computeViewMatrix(pos, pos + camera_vec, up_vec) # 展示圖片 frame = p.getCameraImage(100, 100, view_matrix, proj_matrix)[2] frame = np.reshape(frame, (100, 100, 4)) self.rendered_img.set_data(frame) plt.draw() plt.pause(0.00001)
def __init__(self, root_dir='bull/test_cabinets/solo', masked=False, debug_flag=False): ''' Class for generating simulated articulated object dataset. params: - root_dir: save in this directory - start_idx: index of first image saved - useful in threading context - depth_data: np array of depth images - masked: should the background of depth images be 0s or 1s? ''' self.scenes = [] self.savedir = root_dir self.masked = masked self.img_idx = 0 self.depth_data = [] self.debugging = debug_flag # Camera external settings self.viewMatrix = pb.computeViewMatrix(cameraEyePosition=[4, 0, 1], cameraTargetPosition=[0, 0, 1], cameraUpVector=[0, 0, 1]) # Camera internal settings self.projectionMatrix = pb.computeProjectionMatrixFOV(fov=45., aspect=1.0, nearVal=0.1, farVal=8.1) print(root_dir)
def camera_feed(self): """ Function to get camera feed of the arena. Arguments: None Return Values: numpy array of RGB values """ look = [0, 0, 0.2] cameraeyepos = [0, 0, 6.5] cameraup = [0, -1, 0] self._view_matrix = p.computeViewMatrix(cameraeyepos, look, cameraup) fov = 75 aspect = self._width / self._height near = 0.8 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) img_arr = p.getCameraImage(width=self._width, height=self._height, viewMatrix=self._view_matrix, projectionMatrix=self._proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb = img_arr[2] rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB) return rgb
def _render(self, mode, close): if (mode=="human"): self.isRender = True if mode != "rgb_array": return np.array([]) base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width)/self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage( width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array
def renderPicture(self, height=320, width=320): '''bullet側からカメラ画像を取得''' base_pos, orn = p.getBasePositionAndOrientation(self.hand) 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.01, -0.020, 0.020]) # カメラの座標 # カメラの焦点の座標(ターゲットの座標)、z軸方向を変える。 cam_target = np.array(base_pos) + np.append(target_abs_vec2D, 0.20) 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(width)/height, nearVal=0.1, farVal=100.0) (_, _, rgb, _, mask) = p.getCameraImage( width=width, height=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 step(self,action): p.stepSimulation() start = time.time() yaw = next(self.iter) viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=1,lightDirection=[1,1,1], renderer=p.ER_BULLET_HARDWARE_OPENGL) #renderer=pybullet.ER_TINY_RENDERER) self._observation = img_arr[2] return np.array(self._observation), 0, 0, {}
def test(num_runs=300, shadow=1, log=True, plot=False): if log: logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings") if plot: plt.ion() img = np.random.rand(200, 320) #img = [tandard_normal((50,100)) image = plt.imshow(img,interpolation='none',animated=True,label="blah") ax = plt.gca() times = np.zeros(num_runs) yaw_gen = itertools.cycle(range(0,360,10)) for i, yaw in zip(range(num_runs),yaw_gen): pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix, shadow=shadow,lightDirection=[1,1,1], renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) #renderer=pybullet.ER_TINY_RENDERER) stop = time.time() duration = (stop - start) if (duration): fps = 1./duration #print("fps=",fps) else: fps=0 #print("fps=",fps) #print("duraction=",duration) #print("fps=",fps) times[i] = fps if plot: rgb = img_arr[2] image.set_data(rgb)#np_img_arr) ax.plot([0]) #plt.draw() #plt.show() plt.pause(0.01) mean_time = float(np.mean(times)) print("mean: {0} for {1} runs".format(mean_time, num_runs)) print("") if log: pybullet.stopStateLogging(logId) return mean_time
def _reset(self): """Environment reset called at the beginning of an episode. """ # Set the camera settings. look = [0.23, 0.2, 0.54] distance = 1. pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3) yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3) roll = 0 self._view_matrix = p.computeViewMatrixFromYawPitchRoll( look, distance, yaw, pitch, roll, 2) fov = 20. + self._cameraRandom*np.random.uniform(-2, 2) aspect = self._width / self._height near = 0.01 far = 10 self._proj_matrix = p.computeProjectionMatrixFOV( fov, aspect, near, far) self._attempted_grasp = False self._env_step = 0 self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # Choose the objects in the bin. urdfList = self._get_random_object( self._numObjects, self._isTest) self._objectUids = self._randomly_place_objects(urdfList) self._observation = self._get_observation() return np.array(self._observation)
camDistance = 4 pixelWidth = 320 pixelHeight = 200 nearPlane = 0.01 farPlane = 100 fov = 60 main_start = time.time() while(1): for yaw in range (0,360,10) : pybullet.stepSimulation() start = time.time() viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) stop = time.time() print ("renderImage %f" % (stop - start)) w=img_arr[0] #width of the image, in pixels h=img_arr[1] #height of the image, in pixels rgb=img_arr[2] #color data RGB dep=img_arr[3] #depth data #print(rgb) print ('width = %d height = %d' % (w,h)) #note that sending the data using imshow to matplotlib is really slow, so we use set_data #plt.imshow(rgb,interpolation='none')
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. seg_tiny = np.reshape(images[4],[width,height])*1./255.