
    ni$6                         S r SSKJr  SSKJr  SSKJr  SSKrSSKrSSKrSSKr	SSK
Jr  SrSrS	rS
rSrSr\R$                   " S S\R&                  5      5       rS r\\4S jr SS jr " S S\5      rg)z-Contains APIs to create in sim camera images.    )absolute_import)division)print_functionN)point_cloud_utils
   <   g      Y@g{Gz?)      ?        r
   r
   r
   r	   r
   r
   r
   r
   g     g      r
   r
   g   zr
   )      ?      r   r   c                   ,    \ rS rSrSrSrSrSrSrSr	Sr
g	)

CameraMode   z/Different modes that the camera can operate in.                N)__name__
__module____qualname____firstlineno____doc__RGBDEPTHRGBDPOINTCLOUD_WORLD_FRAMEPOINTCLOUD_ROBOT_FRAME__static_attributes__r       b/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/vision/sim_camera.pyr   r      s$    7	#
%	
$  r!   r   c                      X!-  X"U-
  U -  -
  -  $ Nr   )depthnearfars      r"   from_opengl_depth_to_distancer(   *   s    	sDjE11	22r!   c                    SU R                   R                  -  U-  SSS/nUR                  SSU R                   R                  -  U-  SS/5        UR                  SU R                  R                  -  U-  S-
  SU R                  R                  -  U-  S-
  X4-   * X4-
  -  S/5        UR                  SSSU-  U-  XC-
  -  S/5        [        U5      $ )aY  Converts the camera intrinsics to OpenGL projection matrix.

Args:
  camera_intrinsics: A CameraIntrinsics proto.
  width: Integer. The image width in pixels.
  height: Integer. The image height in pixels.
  far: Float. The far clipping plane distance.
  near: Float. The near clipping plane distance.

Returns:
  An OpengGL projection matrix.

r   r   r   )focal_length_pxxextendyprincipal_point_pxtuple)camera_intrinsicswidthheightr'   r&   projection_mats         r"   &camera_intrinsics_to_projection_matrixr5   .   s    & )99;;;eCQ1M.!//11
1F
:AqAC 

.
.
0
00581<

.
.
0
0069A=
msz"B  Aq3w~<a@A	~	r!   c                 6   U R                  U5      nUSSS2   n[        nUS   US   U-  -   US   US   U-  -   US   US   U-  -   /n	USSS2   n
U R                  XU
5      nU(       a  U R                  OU R                  nU R                  US   US   UUUS9$ )a  Creates a simulated camera image.

Args:
  pybullet_client: A pybullet client.
  camera_position: A list of three floats. The absolute location of the camera
    in the simulated world.
  camera_orientation: A list of four floats. The orientation of the camera in
    world frame, in quaternion.
  resolution: A list of two integers. The horizontal and vertical resolution
    of the camera image in pixels.
  projection_mat: A list of 16 floats. The OpenGL projection matrix, in row
    major.
  egl_render: Whether to use EGL to render the images.

Returns:
  A tuple containing the image resolution and the array for the sythesized RGB
  camera image.
Nr   r   r   r   )
viewMatrixprojectionMatrixrenderer)getMatrixFromQuaternion_DEFAULT_TARGET_DISTANCEcomputeViewMatrixER_BULLET_HARDWARE_OPENGLER_TINY_RENDERERgetCameraImage)pybullet_clientcamera_positioncamera_orientation
resolutionr4   
egl_renderorientation_matforward_vectarget_distancecamera_targetup_vecview_matr9   s                r"   create_camera_imagerK   Q   s    2 $;;<NO/  !$+,/a;q>O;;a;q>O;;a;q>O;;- 14a4 &../57( 
 //(99  
	'	'mm% 
( 
 r!   c                   ~    \ rS rSrSr\\\SS\R                  SS4S jr
S rS rS rS	 r\S
 5       r\S 5       rSrg)MountedCamera   a  A camera that is fixed to a robot's body part.

Attributes:
  resolution: A 2-tuple (width, height) that represents the resolution of the
    camera.
  fov_degree: A floating point value that represents the field of view of the
    camera in the vertical direction. The unit is degree.
FNc                    Xl         X l        X0l        X@l        XPl        X`l        Xl        Xl        Xpl        Xl	        Xl
        Xl        Xl        SU l        Uu  nnUb&  [        XUU R                  U R                  S9U l        gU R                   R!                  U[#        U5      [#        U5      -  U R                  U R                  5      U l        g)aH  Initializes the MounteeCamera class.

Args:
  pybullet_client: A BulletClient instance.
  body_id: Integer. The unique body ID returned from loadURDF in pybullet.
  parent_link_id: Integer. The camera is rigidly attached to a body link
    specified by this ID. For example, camers may be mounted to the base or
    other moving parts of the robot.
  relative_translation: A list of three floats. The relative translation
    between the center of the camera and the link.
  relative_rotation: A list of four floats. The quaternion specifying the
    relative rotation of the camera.
  resolution: A list of two integers.
  fov_degree: The vertical field of view of this camera. Has no effect if
    camera_intrinsics is provided.
  depth_lower_limit: The lower limit of the depth camera.
  depth_upper_limit: The upper limit of the depth camera.
  stabilized: Weather the camera's roll and pitch should be stabilized. If
    turned on, only yaw rotation is applied to the camera.
  camera_intrinsics: A CameraIntrinsics proto instance.
  camera_mode: The mode of the camera. It can be set in the following
    choices [CameraMode.RGB, CameraMode.DEPTH, CameraMode.RGBD].
  normalize_rgb: Whether to normalize the output image pixel values to [-1,
    1].
  egl_render: Whether to use EGL to render the images when the X11 windows
    are not present.
N)r&   r'   )_pybullet_client_body_id_parent_link_id_relative_translation_relative_rotation_resolution_stabilized_camera_mode_fov_degree_near_far_egl_render_normalize_rgb_prev_depthr5   _projection_matcomputeProjectionMatrixFOVfloat)selfr@   body_idparent_link_idrelative_translationrelative_rotationrC   
fov_degreedepth_lower_limitdepth_upper_limit
stabilizedr1   camera_modenormalize_rgbrD   r2   r3   s                    r"   __init__MountedCamera.__init__   s    T ,M)!5/!!#!"J!I!'DME6$C
FLd "22MM

,v
&

DII?dr!   c                 "    U R                  5       $ r$   )render_imagera   s    r"   __call__MountedCamera.__call__   s    r!   c                 |   U R                   S:X  a(  U R                  R                  U R                  5      u  p#O9U R                  R	                  U R                  U R                   SS9nUS   nUS   nU R                  R                  UUU R                  U R                  5      u  pVU R                  R                  / SQU/ SQ[        5      u  px[        UR                  S   5       HR  n	[        UR                  S   5       H3  n
U R                  R                  XXXU
SS24   / SQ5      S   XU
SS24'   M5     MT     U$ )	zProject a sample in the depth image to a 3D point in the world frame.

Args:
  point_cloud: A numpy array of shape (height, width, 3) that represents
    a point cloud in the camera frame.

Returns:
  The transformed point cloud in the world frame.
r*   TcomputeForwardKinematicsr   r   r   r   r   Nr   r   r   r   )rR   rP   getBasePositionAndOrientationrQ   getLinkStatemultiplyTransformsrS   rT   _CAMERA_Z_TO_WORLD_Zrangeshape)ra   point_cloudcamera_link_poscamera_link_oriparent_link_statecamera_position_worldcamera_orientation_world_!camera_space_to_world_orientationijs              r"   0transform_point_cloud_from_camera_to_world_frame>MountedCamera.transform_point_cloud_from_camera_to_world_frame   sO    r!



=
=dmm
L 'o //<<
---- = N)!,o)!,o 	001@151K1K151H1H	J 4 	001I1:1E	G )A
 ;$$Q'([&&q)*!!!44%q!G$l4457 	q!G + ) r!   c                 V   U R                   R                  U R                  U R                  / SQ[        5      u  p#[        UR                  S   5       HR  n[        UR                  S   5       H3  nU R                   R                  UX1XESS24   / SQ5      S   XUSS24'   M5     MT     U$ )zProject a sample in the depth image to a 3D point in the robot frame.

Args:
  point_cloud: A numpy array of shape (height, width, 3) that represents
    a point cloud in the camera frame.

Returns:
  The transformed point cloud in the robot frame.
rv   r   r   Nrw   )rP   rz   rS   rT   r{   r|   r}   )ra   r~   !camera_space_to_robot_translation!camera_space_to_robot_orientationr   r   s         r"   0transform_point_cloud_from_camera_to_robot_frame>MountedCamera.transform_point_cloud_from_camera_to_robot_frame   s     	001K1K151H1H1:1E	G I%
 ;$$Q'([&&q)*!!!4411qQw3G  ! 	q!G + ) r!   c           	      J   / n/ nU R                   S:X  a(  U R                  R                  U R                  5      u  pO9U R                  R	                  U R                  U R                   SS9nUS   nUS   nU R
                  (       a<  U R                  R                  U5      u    pEU R                  R                  SSU/5      nU R                  R                  XU R                  U R                  5      n[        U R                  US   US   U R                  U R                  U R                  S9u    pGp[        R                   " [        R"                  " U5      5      (       a-  [$        R&                  " SXR(                  5        U R(                  nXl        [        R*                  " [        R,                  " U[        R.                  S9U R                  S   U R                  S   S45      n[1        [        R*                  " [        R,                  " U5      U R                  S   U R                  S   S45      U R2                  U R4                  5      n	U R6                  (       a  S	U-  S
-  S-
  nU R8                  [:        R<                  :X  a  USS2SS2SS24   $ U R8                  [:        R>                  :X  a  U	SS2SS2[        R@                  4   $ U R8                  [:        RB                  :X  a  Un
XSS2SS2S4'   U
$ U R8                  [:        RD                  :X  ac  [F        RH                  " XRJ                  S-  [        RL                  -  U	RN                  S   U	RN                  S   5      nU RQ                  U5      nU$ U R8                  [:        RR                  :X  ac  [F        RH                  " XRJ                  S-  [        RL                  -  U	RN                  S   U	RN                  S   5      nU RU                  U5      nU$ [W        SRY                  U R8                  5      5      e)zXRetrieves the camera image.

Returns:
  A simulated view from the camera's perspective.
r*   Trt   r   r   )r@   rA   rB   rC   r4   rD   z6depth contained nan: %s, replacing with prev_depth: %s)dtyper   g     o@Nr   g     f@z"Camera mode {} is not implemented.)-rR   rP   rx   rQ   ry   rV   getEulerFromQuaterniongetQuaternionFromEulerrz   rS   rT   rK   rU   r^   r[   npanyisnanlogginginfor]   reshapearrayfloat32r(   rY   rZ   r\   rW   r   r   r   newaxisr   r   r   distance_map_to_point_cloudrX   pir}   r   r   r   NotImplementedErrorformat)ra   posorir   r   yaw	transformrgbar%   	distances
rgbd_imager~   s               r"   ro   MountedCamera.render_image  s    C
Cr!&&DD
--hc3 //<<
---- = Na ca c))@@Ekq!!!88!QEc%%88$,,d.E.EGI /--!!$Q<##++##%Aq 
vvbhhuollK**,e::bhht2::69I9I!9LdN^N^_`Nace8fgD .


288E?T%5%5a%8$:J:J1:Mr$RSUYU_U_aeajajlIX!dJNN*!Q!)_			j..	.q!RZZ'((			joo	-j%Aq			j??	?%AA
%%-5yq7I
//!
k II
k			j??	?%AA
%%-5yq7I
//!
k II
k D K K


!  r!   c                     U R                   $ r$   )rU   rp   s    r"   rC   MountedCamera.resolutionV      r!   c                     U R                   $ r$   )rX   rp   s    r"   rf   MountedCamera.fov_degreeZ  r   r!   )rQ   rW   r[   rZ   rX   rY   r\   rR   r]   r^   rP   rT   rS   rU   rV   )r   r   r   r   r   _DEFAULT_FOV_DEFAULT_OPENGL_FRUSTUM_NEAR_DEFAULT_OPENGL_FRUSTUM_FARr   r   rl   rq   r   r   ro   propertyrC   rf   r    r   r!   r"   rM   rM      sj      '!=!<!%%>>"??B#J0DL    r!   rM   )F)r   
__future__r   r   r   r   enumginnumpyr   pybullet_envs.minitaur.visionr   r;   r   r   r   #_PYBULLET_DEFAULT_PROJECTION_MATRIXr{   constants_from_enumEnumr   r(   r5   rK   objectrM   r   r!   r"   <module>r      s    3 &  %   
  ; # # 'G # .    3 0K0L	 P $)2hWF Wr!   