
    fQj                        d dl Zd dlZd dlmc mZ d dlmZm	Z	  ej                  d      Zd Zd Zej                  j                  D  ci c]  } |  e|        c} Z ej"                   ej                  dd      j$                        j&                  Zdhd
Zdev rdndZdev rdndZ eee   j0                  d    ee   j0                  d         Z eee   j0                  d    ee   j0                  d         Zej7                  d       ej7                  d       dev rdndZed   ee   cZZej>                  jA                  e      jC                  ejD                  jG                  ed      d          Z$ e%e$jM                               Z' e(d e)e'd             ej0                  \  Z*Z+e+d    Z,ej$                  ej$                  ddd f   e,dz
  kD     Z-e-j]                  d       e-jM                  d       z   dz  Z/ej0                  \  Z0Z1 ejd                  e,d	z
   e&e*d   e0d         dz   e1d   d	z
  g      Z3 ejd                  e0d    d	z    e.e+d   e1d         dz
  e*d   dz   g      Z4 ejj                  e3e4       ejl                  e3e4      cZ7Z8 ee7e8d      Z9e9j7                  d        e(d ejR                  e7d      ju                          ejR                  e8d      ju                                g dZ;dev rdndZ<e;j{                  e<df       d D ]  Z>e>ev se;j{                  e>d!e> d"f         e;D ]  \  Z?Z@ee?   j7                  e@        d dlAZB eBj                  d#d$       d%efd&efd'e9fd(ed(   fdee   ffD ]  \  ZDZEejD                  jG                  eEd      \  ZFZG e%ej>                  jA                  e      jC                  eF      jM                               ZH e(eDd)eEj                  d* e) e&d+eH      d              d,ZJ ej                  d-d.d/i      ZL ej                  eLd0d.d1i        ej                  eLd2d3d4d5      ZN ej                  eNd6d0d1i        ej                  eNd7d0d8i        ej                  eNd9d:d;d<       d=d>d?d@dAdBdBdCdDdEdF
ZOdidGZP ej                  eLd0d.d8i      ZQdHD ]  ZR ePeQeR         ej                  eLd2dIdJd5      ZS ej                  eSd6d0d8i        ej                  eSd7d0dKi        ej                  eSd9 eTeJ      dLz   d:d<        ej                  eSdMdNdOi        ej                  eSdPdQdRdSdSdT        ej                  eLd0d.dKi      ZUdUD ]  ZR ePeUeR eTeJ       dLz   V         ej                   ej
                  eL      dW        ej
                  eL      j                  dXdYdZ[        ej                         Z@g d\ eYd]      j                  j                  d^      rd_gng z   D ]>  \  Z?ZR ej                  eRd      Z\e\j                  d`       e@j                  e\e?e?a       @  ej                  dbd      Z\e\j                  d`       e@j                  e\dcdca       ddD ]?  \  Z?ZR ej                  eRd      Z\e\j                  d`       e@j                  e\e?e?dce       A e@j7                  df        e(dg       yc c} w )j    N)_rrect_stitchzumi_v2_from_blender.glbc           	         t         j                  |    \  }}t        j                  t        j                  t         j
                  |   j                  t        j                  |t                    dz  t         j
                  |   j                        }t        j                  j                  t        j                  |t              d dd df         dk  r|j                          |S )Ng      Y@   r   )sgraphtrimeshTrimeshtransform_pointsgeometryverticesnparrayfloatfaceslinalgdetinvert)nodeTgnms       1/home/cameronsmith/cad_recovery_mnt/gen_umi_v2.pywmeshr      s    GGDMEAr00B1H1H"((STV[J\]`ee

2,,	.A	yy}}RXXa'BQB/014	
H    c                 D    t         j                  j                  | d      S )Nmanifold)engine)r	   booleanunion)ps    r   Ur"      s    &&q&<
<r   
st3215.stlmesh)force      @c                    || z
  }t        t        j                  |            }t        d      D cg c]
  }||k7  s	| }}t        j                  d      |d      t        j                  d      |d      }	}||d      dz  ||d      dz  }}
| |z   dz  }g }t        |      D ]  }||dz
  z  }t        |
|t        ||
dz
  |dz
              }|j                         }| |   ||   |z  z   ||<   |j                  |t        j                  |d d df   |      z   t        j                  |d d df   |	      z           t        |      S c c}w )Nr   r         g      ?)intr   argmaxrangeeyer   mincopyappendouterr   )lohirNLkilate1e2h1h2cringstqr!   s                    r   prism_aarB      sI   
RAS1&%(.MQa1fq.M.MVVAYs1vq	#a& 1Bs1vY]Ac!fIMB	bAAE1X HQK2r3q"s(BH56FFHRUQqTAX-adQ!AqD'2..!AqD'21FFG	H
 5> /Ns
   
EEzCube.001fixed_riserCubefixed_plater(   gnode_v2_fixed_riser.stlgnode_v2_fixed_plate.stlzenpire_moving.001enpire_movingyokei@  z'moving finger vs yoke overlap/gap (mm):r)   g      >@g      @g       @)r4   gnode_v2_yoke_conn.stlzyoke_conn envelope:)holdergnode_v2_holder.stlrI   gnode_v2_yoke.stlenpire_fixedgnode_v2_enpire_fixed.stlgnode_v2_enpire_moving.stl)thumb_cradlefinger_cradle	gnode_v2_z.stlzyoke_gen_thin.stlrO   riserplate	yoke_connrQ   zwt=z vs servo penetration mm:g        g&1robotnameumi_v2linkworldjointworld_to_basefixed)r[   typeparentchildbaseorigin0 0 0z-1.5708 0 0xyzrpyz0.90 0.40 0.30 1z0.30 0.55 0.90 1z0.35 0.35 0.40 1z0.20 0.52 0.85 1z0.55 0.75 0.60 1z0.30 0.66 0.45 1z0.90 0.65 0.20 1z0.55 0.60 0.85 1z0.85 0.30 0.30 1)
gnode_v2_thumb_cradle.stlgnode_v2_handle_column.stlr#   rM   rR   rF   rG   rO   rS   rJ   c           	      4   t        j                  | d      }t        j                  |d|dd       t        j                  t        j                  |d      d|dd       t        j                  t        j                  |d	d
d|z   i      ddt        |   i       y )Nvisualrf   rg   rh   r   r$   z0.001 0.001 0.001)filenamescalematerialr[   r   colorrgba)ET
SubElementCOLS)r]   fnoffvs       r   visrz   P   sz    
dH%Ar}}Q#V]B^'_MM"--:.RRe8fgMM"--:b/ABGfVZ[]V^M_`r   )r#   rM   rR   rF   rG   rl   jawrevolutejaw_linkz 0 0axisri   z0 1 0limitz-0.9z0.13)loweruppereffortvelocity)rO   rJ   rS   rk   )rx   z  zumi_gripper_v2.urdfunicodeT)encodingxml_declaration))servor#   rK   rN   )rT   rk   )rU   zgnode_v2_finger_cradle.stlrP   )rH   rS   )rC   rF   )rE   rG   )rY   rJ   )cam_connzgnode_v2_cam_conn.stl)
aruco_cubezgnode_v2_aruco_cube.stlosrl   )handle_columnrl   g{Gz?)	node_name	geom_namezgnode_v2_camera_mount.stlcamera_mount))camera_bodyzgnode_v2_camera_body.stl)camera_coverzgnode_v2_camera_cover.stl)r   r   parent_node_namezumi_gripper_v2.glbzwrote umi_gripper_v2.urdf + glb)r&      )rg   )_numpyr   r	   xml.etree.ElementTreeetreeElementTreert   	gen_curvyr   r   loadr   r   r"   r   nodes_geometryM
PointCloudr   convex_hull
servo_hullrB   r_keyp_keyboundsrW   rX   exportmv_keyykmv	proximityProximityQuerysigned_distancesamplesample_surfacedr   maxoverlapprintroundrb_lorb_hiroot_x1rootr.   rcyb_loyb_hir   r2   r3   minimummaximumlo2hi2conntolistexportsmv_namer0   cnr   outshutil_shr/   r[   r$   pts_penis_watertightAXElementRru   wjrv   rz   re   rw   jjstrjlindentwriteScene
__import__pathexistsr   apply_scaleadd_geometry)ns   0r   <module>r      sH    " " %GLL*+ =''001QQa[1W\ H Q QR^^
 !Ao
=A+=5#QuX__Q%785#QuX__Q%78 ' (%,,7Q*R 3q 8	o	
6AfIB$$R(889V9VWY[_9`ab9cd
. /w1B Cyyu
(	{{2;;q!t$w~56
hhqkDHHQK1$yyuRXXw}c%(E!H5;U1X^LMRXXuQx#~s58U1X6<eAhnMN2::b"zrzz"b1SSC  $ % XRXXc1-446a8H8O8O8Q R:!4!9
 56 7
+ ;B	Qwit4$89:; ID#dGNN3  	1 2U#gu%5T7J"An$56!F)8TV aJD$^^**46FC
!!00<LLSQUUW
XC	$t))+FcRUWZm]^H_`	a BJJw*+]R]]1fvwFW-XR]]1gIJ b(VW- .b'FTZK[0\ b(GMB C%7Wi(AS%7Ug$6M_&8Tf	h
a r}}Q 01 c BbMR]]1gzBC b(VV, -}r}}R6S]J^/_ b(CGf$4WE FVXZ`chjqbrHs b'VesX[\ ]R]]1fvz23
t 'BBCHv%&' 			."..
T "NBNN1$5$;$;<Q\ew{$;$|gmmoc GQQUFVF[F[FbFbc  GAAB  GIJ 8HD" 	Rv&Ad(;Q$$78 GLL,F;Q]]4=P   n  Gl YHD"Rv&Ad(;Q$$XY 

   ' (C 2s   
[*