o
    ŒwÕi;T  ã                   @   sD  d Z ddlZddlZddlZddlZddlmZ ddl	m
Z
mZmZ ddlmZ ddlmZ e ej¡Zd:dd„Zdejd	ejfd
d„Zdejd	ejdedejfdd„Zd;dejd	ejde
eejf fdd„Zd<dejdedejfdd„Zd<dejdejd	ejdedejf
dd„Zd=dejdejd	ejdejdejdedeej fdd„Zd=dejdejd	ejdejdejdedeej fdd„Z dejd	ejdejfd d!„Z!d>dejd	ejd#e"d$edejf
d%d&„Z#d'ed(edefd)d*„Z$d=dejd	ejdejdejd+e%d,e%d-efd.d/„Z&	0	1	2	"	d?dejd	ejd3e%d4e"d5e"d6ed7efd8d9„Z'dS )@z7Utilities for exoskeleton-based robot state estimation.é    N)ÚRotation)ÚDictÚTupleÚOptional©Úarucoc	           +   
   C   s®  ddl m}	 t  | t j¡ tj¡}
|du r$|	 ||	 ¡ ¡ 	|
¡\}}}n|\}}}|du r/dS t j 
|du r=t  |
t j¡n|||¡}| jdd… ddd… \}}t||ƒd }|du retjdtjd }}|du rÍtj|d|d gd||d gg d	¢gtjd}t jt jB t jB t jB t jB t jB t jB t jB t jB }| ||¡\}}t j| dd
d¡ tj¡g| dd
d¡ tj¡g|
jddd… |||d\}}}}}| ||¡\}}|du sÞ|jdkràdS | dd¡ tj¡}| dd¡ tj¡}t j||||t j d\}}}}d}t!t"||ƒƒD ]-\}\}} t  #|¡\}!}|!t g d¢¡ }"| d dk}#|"d dk }$|#r8|$r8|} nq|| dd…df || dd…df }%}&t  #|%¡d |j$ |& dd
¡ j$}'t  %||%|&||¡\}(}|( dd¡}(t  #|%¡d }!tj|d |d dgtjd})|&|! &|)¡ }&t  '||t d¡|%|&d¡}t (d¡}*|&|*dd…df< |!|*dd…dd…f< |*dd…d
d…f  d9  < |*||| )¡ |f||%|&f||||f|'|fdœ	S )z2Estimate ArUco pose - copied from aruco_helpers.pyr   r   Néÿÿÿÿé   )é   é   ©Údtypeg       @)r   r   ç      ð?r   é   )Úflags)ç        r   r   )r   é   ç¸…ëQ¸ž?é   )	Úest_aruco_poseÚpose_visÚcorners_visÚcornersÚcameraMatrixZrtvecÚ
distCoeffsÚcorners_estÚobj_img_pts)*Úcv2r   ÚcvtColorÚCOLOR_BGR2GRAYÚastypeÚnpÚuint8ÚArucoDetectorÚDetectorParametersÚdetectMarkersÚdrawDetectedMarkersÚCOLOR_GRAY2BGRÚshapeÚmaxÚzerosÚfloat64ÚarrayÚCALIB_ZERO_TANGENT_DISTÚCALIB_FIX_K1ÚCALIB_FIX_K2ÚCALIB_FIX_K3ÚCALIB_FIX_K4ÚCALIB_FIX_K5ÚCALIB_FIX_K6ÚCALIB_FIX_ASPECT_RATIOÚCALIB_FIX_PRINCIPAL_POINTÚmatchImagePointsÚcalibrateCameraÚreshapeÚfloat32ÚsizeÚsolvePnPGenericÚSOLVEPNP_IPPEÚ	enumerateÚzipÚ	RodriguesÚTÚprojectPointsÚdotÚdrawFrameAxesÚeyeÚflatten)+ÚframeÚ
aruco_dictZboardÚboard_lengthr   r   r   r   r   r   Úgrayr   ÚidsÚrejectedÚWÚHÚf0Zdist_coeffs_initZcamera_matrix_initr   Zobj_pts_calibZimg_pts_calibÚretÚrvecsÚtvecsÚobj_ptsÚimg_ptsÚokÚ_Zbest_idxÚiÚrvZtvÚR_matÚnormalZboard_in_frontZfaces_cameraÚrvecZtvecZobj_pts_camZ
img_reprojZcenter_offset_boardr   © r[   úA/data/cameron/para/panda_streaming/hand_eye_calib/../exo_utils.pyÚdo_est_aruco_pose   sh   &
&.ÿÿÿÿþT€*"
.r]   ÚmodelÚdatac           	      C   sš   i }| j  ¡ D ]C\}}t |tjj|j¡}|j| }tj	 
|¡dk r#qt d¡}t |g d¢ ¡ ¡ |dd…dd…f< |j| |dd…df< |||< q|S )z3Extract link poses from robot's current state.
    ç{®Gáz„?r   ©r   r	   r   r   Nr   )ÚlinksÚitemsÚmujocoÚ
mj_name2idÚmjtObjÚ
mjOBJ_BODYÚpybullet_nameÚxquatr!   ÚlinalgÚnormrD   ÚRÚ	from_quatÚ	as_matrixÚxpos)	Úrobot_configr^   r_   Ú
link_posesÚ	link_nameÚcfgÚbody_idÚ	quat_wxyzÚposer[   r[   r\   Úget_link_poses_from_robotK   s   

&
rw   Ú	body_nameÚreturnc                 C   s    t  | t jj|¡}|dk rtd|›ƒ‚|j| }tj |¡dk r(td|›ƒ‚tj	dtj
d}t |g d¢ ¡ ¡ |dd	…dd	…f< |j| |dd	…d	f< |S )
zN4x4 homogeneous world pose of the named body origin (MuJoCo `xpos` / `xquat`).r   zBody not found: r`   zInvalid quaternion for body r   r   ra   Nr   )rd   re   rf   rg   Ú
ValueErrorri   r!   rj   rk   rD   r+   rl   rm   rn   ro   )r^   r_   rx   rt   ru   rv   r[   r[   r\   Úget_body_pose_in_world_   s   
&r{   rq   c                 C   s   | j  ¡ D ]Â\}}|durB||v rB|| }|dd…df }|dd…dd…f }tjtj |¡ddds4qt |¡}	|	 ¡ g d¢ }
n"| 	|j
¡j}|j| }|j| }
zt |
g d¢ ¡}	W n   Y q|jt |tjj|› d¡ }||j|< |
|j|< |jt |tjj|› d	¡ }||j|< |
|j|< t |tjj|› d
¡}||	 |jd ¡ |j|j| < |	t d|j¡  ¡ g d¢ |j|j| < qt ||¡ dS )z$Position virtual exoskeleton meshes Nr   r   r`   )Úatol©r   r   r   r	   ra   Z
_link_meshZ	_exo_meshZ
_exo_planeéè  Úxyz)rb   rc   r!   Úallcloserj   Údetrl   Úfrom_matrixÚas_quatÚbodyrh   Úidro   ri   rm   Úbody_mocapidrd   re   rf   rg   Ú	mocap_posÚ
mocap_quatÚapplyÚaruco_offset_posÚ
from_eulerÚaruco_offset_rotÚ
mj_forward)rp   r^   r_   rq   rr   rs   Z	link_poseZlink_posZrot_matZlink_rotZlink_quat_wxyzrt   Zlink_mocap_idZexo_mesh_mocap_idZplane_body_idr[   r[   r\   Úposition_exoskeleton_mesheso   s.   







 ,rŽ   FÚrgbÚ	visualizeÚcam_Kc              
   C   s˜  | j tjks| j tjkr| d  tj¡} t | tj¡}tj	 
ttj	 ¡ ¡ |¡}|  ¡ }|\}}}	tj	 |||¡ i }
i }|j ¡ D ]q}|j| }t| t|j| |j|||d}|dkr]qB|d }t t d|j¡ ¡ |jd d …d f d gt d¡dgg¡}|d	 tj |¡ |
|< |d
 ||< |dkr§|d |
| t d¡}}|
d< qBtj |¡|
|  |
|< qB|rÄt  t |tj!¡¡ t "¡  |
|||||fS )Néÿ   ©r   r   r   r   r   r   r~   ©r   r   r   r   r   Úlarger_baser   r   )#r   r!   r9   r+   r    r"   r   r   ÚCOLOR_RGB2GRAYr   r#   Ú
ARUCO_DICTr$   r%   Úcopyr&   rb   Úkeysr]   Úaruco_board_objectsrH   Úblockrl   r‹   rŒ   rn   rŠ   r*   rj   ÚinvrD   ÚpltÚimshowÚCOLOR_BGR2RGBÚshow)r   rp   r   r‘   rI   Úcorners_cacheÚvis_imgr   rJ   rK   rq   r   rr   rs   ÚresultZlink_to_arucoÚcam_poser[   r[   r\   Údetect_link_poses™   s(   (


<, r¥   c                 C   s:   t | |||d\}}}}}	}
t||||ƒ |||||	|
fS )zZDetect link poses from ArUco markers in image and set virtual exoskeleton meshes to match.)r   r‘   )r¥   rŽ   )r   r^   r_   rp   r   r‘   rq   Zcamera_pose_worldr¡   r   r   r[   r[   r\   Údetect_and_set_link_poses¼   s   r¦   r¤   c              	   C   s¤  | j tjks| j tjkr| d  tj¡} |  ¡ }t| t|j	|j
|||d}	|	dkr+dS |r=t t |	d tj¡¡ t ¡  t t d|j¡ ¡ |jdd…df gt d¡dgg¡}
|	d	 tj |
¡ }tj |¡| }| d
¡j}|dd…df |j|j| < t |dd…dd…f ¡ ¡ g d¢ |j |j| < t! "|t!j#j$d¡}||
 }|dd…df |j|j| < t |dd…dd…f ¡ ¡ g d¢ |j |j| < ||	d fS )zðDetect puck pose from ArUco and position it in the scene.
    
    Args:
        puck_config: PuckConfig object with ArUco board and offset info
    
    Returns:
        Puck pose (4x4 matrix) relative to base, or None if not detected
    r’   r“   r   Nr   r   r”   r   r   Úgrabbable_puckr   r}   Úgrabbable_aruco_planer   )%r   r!   r9   r+   r    r"   r˜   r]   r—   Úaruco_boardrH   r   rž   r   r   rŸ   r    r›   rl   r‹   rŒ   rn   rŠ   r*   rj   rœ   r„   r…   r‡   r†   r‚   rƒ   rˆ   rd   re   rf   rg   )r   r^   r_   Úpuck_configr‘   r¤   r¡   r   r¢   r£   Úpuck_to_arucoZpuck_pose_cameraÚ	puck_poseÚpuck_body_idZaruco_body_idÚ
aruco_poser[   r[   r\   Údetect_and_position_puckÃ   s    ($822r¯   c              	   C   s<  | j tjks| j tjkr| d  tj¡} |  ¡ }t| t|j	|j
|||d}	|	dkr+dS |r=t t |	d tj¡¡ t ¡  t t d|j¡ ¡ |jdd…df gt d¡dgg¡}
|	d	 tj |
¡ }tj |¡| }| d
¡j}|dd…df |j|j| < t |dd…dd…f ¡ ¡ g d¢ |j |j| < ||	d fS )a¯  Detect alignment board pose from ArUco and position it in the scene.
    
    Args:
        alignment_board_config: AlignmentBoardConfig object with ArUco board and offset info
        cam_K: Camera intrinsic matrix
        cam_pose: Camera pose in world frame (4x4 matrix)
        corners_cache: Pre-detected ArUco corners from image
    
    Returns:
        Board pose (4x4 matrix) relative to base, or None if not detected
    r’   r“   r   Nr   r   r”   r   r   Úalignment_boardr   r}   r   )!r   r!   r9   r+   r    r"   r˜   r]   r—   r©   rH   r   rž   r   r   rŸ   r    r›   rl   r‹   rŒ   rn   rŠ   r*   rj   rœ   r„   r…   r‡   r†   r‚   rƒ   rˆ   )r   r^   r_   Zalignment_board_configr‘   r¤   r¡   r   r¢   r£   Zboard_to_arucoZboard_pose_cameraZ
board_poseZboard_body_idr[   r[   r\   Ú#detect_and_position_alignment_boardê   s   ($82r±   c           
      C   s  |   d¡j}| j| }|j| }|j| }t t |g d¢ ¡ 	¡ |dd…df gt 
d¡dgg¡}t t d|j¡ 	¡ |jdd…df gt 
d¡dgg¡}|| }	|	dd…df |j| j|   d¡j < t |	dd…dd…f ¡ ¡ g d	¢ |j| j|   d¡j < |	S )
a  Position ArUco marker based on puck's current mocap pose.
    
    Args:
        model: MuJoCo model
        data: MuJoCo data
        puck_config: PuckConfig object with ArUco offset info
    
    Returns:
        ArUco pose (4x4 matrix) in world frame
    r§   ra   Nr”   r   r   r   r¨   r}   )r„   r…   r†   r‡   rˆ   r!   r›   rl   rm   rn   r*   r‹   rŒ   rŠ   r‚   rƒ   )
r^   r_   rª   r­   Zpuck_mocap_idZpuck_posZpuck_quat_wxyzr¬   r«   r®   r[   r[   r\   Úposition_puck_aruco_from_mocap  s   


:8$:r²   TÚdtÚvisualize_targetc                 C   sž  t  t dg d¢¡ ¡ t ddt jdg¡ ¡  t  g d¢¡dd…df  gt  d¡dgg¡}t| ||ƒ}|| }|rjt	 
| t	jjd¡}	|dd	…d	f |j| j|	 < t |dd	…dd	…f ¡ ¡ g d
¢ |j| j|	 < tjddddd}
|
 tjt  t |dd	…dd	…f ¡ ¡ g d
¢ |dd	…d	f g¡d¡ tj| dd}| |j¡ tj||
|g|dt | ¡gd}| ||¡ |j|jdd…< t	 | |¡ t j |¡S )z4Use IK to track ArUco marker with fixed offset.
    r   )gñÔÈSû!	ÀgÈÎõ¾ù?gxö®¤™ù¿r   )go³¡0b~‚?g®0ID|p?gCÅ8
‘¿Nr”   r   Útargetr   r}   Zend_effectorÚsitegš™™™™™¹?©Z
frame_nameZ
frame_typeÚposition_costÚorientation_cost©Zwxyz_xyzgü©ñÒMbP?)ÚcostÚdaqp©Úlimits) r!   r›   rl   r‹   rn   Úpir,   r*   r²   rd   re   rf   rg   r‡   r†   r‚   rƒ   rˆ   ÚminkÚ	FrameTaskÚ
set_targetÚSE3ÚconcatenateZPostureTaskÚqposÚsolve_ikÚConfigurationLimitÚintegrate_inplaceÚqr   rj   rk   )r^   r_   rª   Úconfigurationr³   r´   Zaruco_to_ee_offsetr®   Ztarget_ee_poseZtarget_body_idZee_taskZposture_taskÚvelr[   r[   r\   Útrack_aruco_with_ik*  s    \2J rÌ   Úbase_xmlÚ	additionsc                 G   s"   | }|D ]
}|  d|d ¡}q|S )Nz	</mujoco>z

</mujoco>)Úreplace)rÍ   rÎ   ÚxmlZadditionr[   r[   r\   Úcombine_xmlsO  s   rÑ   ÚheightÚwidthÚsegmentationc           
   	   C   sÊ   ddl m} || ||d}|  d¡j}	|r| ¡  tj |¡dd…df |j|	< t 	g d¢g d¢g d	¢g¡|dd…dd…f  j
 d
¡|j|	< t dt |d|d   ¡ ¡| j|	< |j||	d | ¡ S )zdRender scene from estimated camera pose.
    
    Returns:
        Rendered image (numpy array)
    r   )ÚRenderer)rÒ   rÓ   Zestimated_cameraNr   )r   r   r   )r   r   r   )r   r   r   r   r	   )r   r   )Úcamera)Úmujoco.rendererrÕ   Úcamr…   Úenable_segmentation_renderingr!   rj   rœ   Úcam_xposr,   r@   r8   Úcam_xmatÚdegreesÚarctanÚcam_fovyÚupdate_sceneÚrender)
r^   r_   r¤   r‘   rÒ   rÓ   rÔ   rÕ   ÚrendererÚcam_idr[   r[   r\   Úrender_from_camera_poseU  s   >&rã   é   r   r   Úik_iterationsr¸   r¹   Úenforce_limitsÚreturn_errorc	                    s>  t  | ¡}	|	 |j¡ g }
g }dd„ |j ¡ D ƒ}|D ]\‰ ˆ | ¡ vr$q‡ fdd„| ¡ D ƒd }t j|d||d}t 	|ˆ  ¡}t
 |dd	…dd	…f ¡ ¡ g d
¢ }|dd	…d	f }| t jt ||g¡d¡ |
 |¡ | ˆ ||f¡ q|rt j| dgng }t|ƒD ]}t j|	|
dd|d}|	 |d¡ q‡|	j|jdd…< t | |¡ g }g }|D ]N\‰ }}|  |¡j}|j| }|j| }|dd	…d	f }t
 |dd	…dd	…f ¡}t
 |g d¢ ¡}| dtj || ¡ ¡ | t ||  ¡   !¡ ¡¡ q®|rt"t #|¡ƒnd|rt"t #|¡ƒnddœ}|r|	|fS |	S )a%  Estimate robot joint configuration from link poses (e.g. fixed_gripper, moveable_gripper).

    Uses IK to fit the two gripper link poses. When enforce_limits=False, joint limits are
    not applied so the solution can match the detected poses even if they are not reachable
    by the real robot (useful for visualization or training targets).

    Returns:
        configuration: mink.Configuration with solved joint state
        ik_error: dict with "position_mm" (mean position error in mm) and "orientation_deg" (mean angular error in deg)
    c                 S   s   i | ]\}}|j |“qS r[   )rh   )Ú.0rr   rs   r[   r[   r\   Ú
<dictcomp>†  s    z(estimate_robot_state.<locals>.<dictcomp>c                    s   g | ]
\}}|ˆ kr|‘qS r[   r[   )rè   ÚkÚv©rr   r[   r\   Ú
<listcomp>Š  s    z(estimate_robot_state.<locals>.<listcomp>r   r„   r·   Nr   r}   rº   )r^   g{®Gázt?r¼   r½   ra   g     @@r   )Zposition_mmZorientation_deg)$rÀ   ZConfigurationÚupdaterÅ   rb   rc   ÚvaluesrÁ   r!   Úasarrayrl   r‚   rƒ   rÂ   rÃ   rÄ   ÚappendrÇ   ÚrangerÆ   rÈ   rÉ   rd   r   r„   r…   ro   ri   rm   rj   rk   rÜ   rœ   Ú	magnitudeÚfloatÚmean)r^   r_   rp   rq   rå   r¸   r¹   ræ   rç   rÊ   ZtasksZ	link_infoZlink_name_mapZpyb_nameZtaskrv   ru   r   r¾   rU   rË   Zpos_errors_mmZorient_errors_degZtarget_posert   Úcurrent_posZcurrent_quat_wxyzZ
target_posZ
target_rotZcurrent_rotZik_errorr[   rì   r\   Úestimate_robot_statel  sV   
ü&


þr÷   )NNNNN)N)FN)F)T)rä   r   r   TF)(Ú__doc__r   Únumpyr!   rd   rÀ   Úscipy.spatial.transformr   rl   Útypingr   r   r   r   Úmatplotlib.pyplotÚpyplotr   ÚgetPredefinedDictionaryÚDICT_6X6_250r—   r]   ÚMjModelÚMjDatarw   ÚstrÚndarrayr{   rŽ   Úboolr¥   r¦   r¯   r±   r²   rô   rÌ   rÑ   Úintrã   r÷   r[   r[   r[   r\   Ú<module>   sd    
=ÿÿÿ
þ&*(#88'$&%0÷ÿþûúùø	÷