ij L d Z ddlZej dd ddlZddlZddlZddlmZ ddl Z ddl
Z
ddl Z ddl
mZ e Z ed Zd Z ee Zd Zd
Zd Zd Zd
Zd Zd Zd Zd ZddZ d Z e dk r e e dS )aY YAM bimanual silhouette overlay renderer.
Renders both arms' silhouettes using MuJoCo from a single MJCF (one arm),
re-using the same model by injecting a camera and updating qpos. For the right
arm, the camera is brought into right_arm_base coords via inv(T_left_from_right).
Output: a side-by-side panel (raw | overlay) per requested frame.
N MUJOCO_GLegl)Path) get_yam_4310_linear_xml_pathz /home/robot-lab/data/processedc t t | }t | j }i }|S )zBLoad mesh assets from all directories the combined XML references.)r read_textparent)xml_pathxmlxml_dirassetss 9/home/robot-lab/cameron/yam_control/yam_overlay_render.py load_assetsr s3
x..
"
"
$
$C8nn#G
F M c
t | d }t dt j t j |d|z z z S )N) r )floatnpdegreesarctan)KHfys r fovy_from_Kr + s@ qw B RZ !q2v, 7 7888 9 99r c t j g d }||z }|dddf |dddf } }d|d dd|d dd|d dd |d dd|d dd|d dd| d dd| d dd| d dd
|d d }
d
| d| d} t | } | d| dd } | dd|
d } | S )z\Inject a fixed pinhole camera (OpenCV convention) + offscreen buffer size into the YAM MJCF.)r r Nr r zzzz
z z z
)r diagr r replace)
r
cam_pos
cam_rot_cvfovyWr opencv_to_mj
cam_rot_mjcam_xcam_ycam_str
visual_strr s
r build_xml_with_camerar+ 0 s 7;;;''Ll*J aaad#Z1%55E WQZY gajY wqzY 8@ "'(@ 16q@ 89 (9 ).q9 NaMMaMMMJ
x..
"
"
$
$C
++iJ!;!;!;Q
?
?C
++n&D7&D&D&D
E
EC Jr c |dddf }|ddddf }t || }t t ||||| }t j | } t j | }
t dt | } | d| |
j d| < t | dk rK| j
dk r@t t j
| d dd dz } | |
j d< | |
j d< t j | |
t j | || }
t j | t j j d
}|
|
|
| |
}|
t | }||d z k t j }||fS )
zRender a single arm + gripper at world origin from a given camera pose. Returns (mask, depth).
joints7: (7,) array. First 6 are arm joints; 7th is gripper opening.
N g g ?gR Q?)heightwidthposed)camerag+?)r r+ YAM_XMLmujocoMjModelfrom_xml_stringMjDataminlenqposnqr r clip
mj_forwardRenderer
mj_name2idmjtObj mjOBJ_CAMERAenable_depth_rendering update_scenerendercopyclosemaxastypeuint8)joints7
T_cam2baser r$ r r! cam_rotr# r modeldataarm_nslider_mrenderercam_iddepthfarmasks r render_arm_maskrX B s
!Q G!RaR G q! D
'4A
F
FC N * *3 / /E = D 3w<< E DIfuf 7||qUX]]S#6677&@ ! !
eT"""uQa888H
ufm&@'
J
JF ##%%% $v... OO " " $ $E NN
C C%K ' ' 1 1D ;r c t | d d |||| \ }}t j | }||z } t | dd | ||| \ }
}||
fS )Nr/ )rX r linalginv) joints14 T_cam2worldT_left_from_rightr r$ r mask_l_T_inv T_cam2rbasemask_rs r render_bimanualre g sl ! k1aCCIFA IMM+ , ,E+%K 2 Q1EEIFA 6>r c | }|df|dffD ]\ }}|| dk r t j |t j t j \ }}t j ||d|d | }t j ||| t j |d|dd }|S )N)r r )r r rg r r r g ?g ?) rG sumcv2 findContours
RETR_EXTERNALCHAIN_APPROX_SIMPLE drawContoursfillPoly addWeighted) imgr` rd outrW colorcsra fills r overlay_contoursru p s
((**C - /DE 8 8 e <488::?? s'8#:QRRA b"eQ///xxzz T2u%%%oc4tQ77 Jr c t | } | D ]"}| }d|v rd|vr|c S #| D ] }d| vr|c S | r| d ndS )zCcams: iterable of camera names. Returns the scene cam name or None.scenewristr N)listlower)camsccls r find_scene_camr~ ~ s ::D
WWYY b==WB . .HHH
!'')) # #HHH $ $477$r c X t d | D S )Nc h g | ]/}| |d z -|0S )lowdim)is_dir).0ds r
z!list_episodes.. s: [[[AHHJJ[ALCXCXCZCZ[1[[[r )sortediterdir)task_dirs r
list_episodesr s+ [[h . . 0 0[[[ \ \\r c n t | dz d }t | |fS )Nr z*.pkl)r globr; )ep_dirpklss r count_framesr s2 6H$**733 4 4D t99d?r c f |d}dD ])}| dz |z | | z }| r|c S *d S )N010d)z.jpgz.pngz.jpegrgb)exists)r cam frame_intfstrextcands r
find_rgb_pathr sZ D( ~#nsnn4 ;;== KKK 4r c
t | z }||z }t | \ }} |dk rd d| fS ||dz }|d}
|dz |
dz } t | d 5 } t j | }
d d d n # 1 swxY w Y |'t |
d }t ||| }|d d | fS t j |
d
}t j |
d }|
d
| }|
d
| }||d d
| fS t j | }t j | }t j t | }|j d d \ }}t |||||| \ }}t! ||| }t j ||gd }t j ||df||fdd |p| d| d| d| d| } t j || dt j dddt j |rKt- | j dd t j t | | || fS )Nr z
no frames in r r r z.pklrb
intrinsicszmissing rgb for cam jointsr_
extrinsicszno extr/intr for cam r )axis)( r r /z cam=z f=)
gffffff?)rg rg rg T)parentsexist_ok) DATA_ROOTr openpickleloadr~ keysr r arraygetri imreadstrshapere ru concatenatelineputTextFONT_HERSHEY_SIMPLEXLINE_AAr r mkdirimwrite) taskepr frameout_pathlabelr r n_framesr r pkl_pathffdrgb_pathr T_lfrextrintrr r^ rp r r$ r` rd overlaypanellbls r
render_oner s 4 H
]F!&))NHd1}}-V---- }A
??D d===0H
h
[^^
{ R - 2 2 4 455 VS%00H1C1111
Xbl
#
#F HR+, - -E
l $ $D
l $ $D |t|2S2222
A(4..K
*S]]
#
#C 9RaR=DAq$V[%AqIINFF sFF33G NC> 2 2 2EHUQFQFL!444
@d@@R@@c@@e@@h@@CKsHc&>_VWY\Ydeee * X ##D4#@@@ CMM5))) #:s A66A:=A:c
t j } | dd | dd | dd d | d
t d d | d
d | }t |j |j |j |j |j
\ }}|t d| dS t d|j
d| d dS )Nz--taskT)requiredz--ep0000)defaultz--camz#defaults to scene cam from metadata)r helpz--framezdefaults to middle frame)typer r z--outzFAIL: r z
OK: saved z ()r ) argparseArgumentParser add_argumentint
parse_argsr r r r r rq print)pargsr r s r mainr s !!ANN8dN+++NN66N***NN7D/TNUUUNN93;UNVVVNN7TN*** <<>>D di$(DJQQLE5 }
u q
,tx
,
,E
,
,
,--- 1r __main__)NNNN) __doc__osenviron
setdefaultr jsonr pathlibr ri r6 numpyr raiden._xml_pathsr r5 r r
YAM_ASSETSr r+ rX re ru r~ r r r r r __name__
SystemExit r r r s
k5 ) ) )
; : : : : :
&
&
(
( D1 2 2 [
!
!
: : :
$" " "J % % % ] ] ]
' ' ' 'T
z
*TTVV
r