
    )Dj#                     B   d Z ddlZddlZddlmc mZ d\  ZZ	d\  Z
ZdZdZd\  ZZdZd	Zd
Zedz   Zeez
  ZdZdZdZdZdZdZg dZd\  ZZd Zd Zd Z d Z!d Z"d Z#d0dZ$d1dZ%e&dk(  r4 e"        e#       cZ'Z(e'jS                  d       e(jS                  d        ejT                  dd      Z+ejX                  j[                  e+e(gd !      Z.e.jS                  d"        e/e.ja                  d#$            Z1 e2d% ejf                  e'jh                  d&      jk                          d'e'jl                   d( ejf                  e(jh                  d&      jk                                  e2d)e.jn                  d*z  d+d,e.jl                   d-e1 e1d&kD  rd.nd/         e%        yy)2u  Parametric thumb-style gripper (path B) — STANDALONE test rig, NOT wired into the main arm yet.
One servo: the horn drives a thumb finger that closes against a fixed jaw mounted on the holder.

Servo frame (mm): output axis +Y through (X=AX=-25.5, Z=0); horn on +Y face (Y=9.6), idler -Y (Y=-28.2).
Fingers extend in -X and close in Z (rotation about +Y). Flush-at-closed is automatic: both fingers are
authored at the CLOSED pose (thumb pad at +GRIP_GAP/2 facing -Z, fixed pad at -GRIP_GAP/2 facing +Z, parallel)
and the jaw joint OPENS by rotating the thumb up about the output axis.

Reuses st3215.stl (servo) + holder_approx_drilled.stl (holder). Outputs thumb.stl, fixed_jaw.stl,
gripper_testing.urdf. Iterate via gripper_render.py (open vs closed).
    N)     9        )g333333#@g333333<      @g      N@)g      &@g      ?@g      @vgrooveg      @g?finrayg      0@       @   ))g     @@r   )r   g      @)r   g      )g     2r   )g333333@g      "@c                     t         j                  j                  || z
  ||z
  ||z
  g      }|j                  | |z   dz  ||z   dz  ||z   dz  g       |S )Nextents   )trimeshcreationboxapply_translation)x0x1y0y1z0z1bs          6/data/cameron/repos/smith300_para_stuff/gripper_gen.py_boxr   ,   s`    b2grBwR%@AA"r'QbAR1}EFH    c                    t         j                  j                  |dz  ||z
  d      }|j                  t         j                  j                  t        j                  dz  g d             |j                  | ||z   dz  |g       |S )zTA cylinder of diameter `dia` along the Y axis, centred at (x, z), spanning Y[y0,y1].r       )radiusheightsectionsr      r   r   )	r   r   cylinderapply_transformtransformationsrotation_matrixnppir   )xzdiar   r   cs         r   _cyl_Yr-   2   sr    !!s27R!PAg--==beeaiSTR"WOQ/0Hr   c                  T   t         t        dz   }} t        dd| |dd      }t        D cg c]  \  }}t	        ||t
        | dz
  |dz           }}}|j                  t	        t        t        t        | dz
  |dz                t        j                  j                  |g|z   d      S c c}}w )	zReal horn mount: a plate on the horn face (Y=9.6..) drilled with the exact 4-bolt pattern + center
    bore, so the thumb bolts precisely to the STS3215 output horn (the 'precise mating' ask).r   i   r"   manifoldengine)HORN_YFY0r   HOLES_XZr-   BOLT_DappendAXAZBORE_Dr   boolean
difference)r   r   platehxhztoolss         r   _horn_platerC   :   s     S3YBc2r3+EBJK(BVBFBFBF3(EK	LLBQQ78??%%ugoj%II Ls   #B$c                 L   ddl m}m} ddlm}  |d| dfd|fg      }|j                  |j                  |             g}t        d|dz         D ]L  }	| |	z  |dz   z  }
|
dz  }|d|| z  z
  z  }|j                   ||
df||fg      j                  |dz               N  ||      S )	z2D fin-ray outline in (length u, height v): front (contact) edge along v=0, base along u=0, spine =
    hypotenuse (0,H)->(L,0). Hollow outer walls + angled internal ribs. Returns a shapely polygon.r   )Polygon
LineString)unary_union)r   r   r"   g?r   r   )	shapely.geometryrE   rF   shapely.opsrG   r>   bufferranger9   )LHwalln_ribsrE   rF   rG   tripartskx_fx_sy_ss                r   _finray_profilerV   D   s     5'
6Aq6Aq6*
+C^^CJJu-./E1fqj!!evz"Dj1sQw;Z#sc3Z 89@@LM	 "
 ur   c            	      j   t         dz  } t               }t        ddt        t        dz   | d      }t        dk(  rt        t        t        t        t              }t        j                  j                  |t        t        z
        }t        j                  dddt         gddd	t        gdd	d| gg d
gt"              }|j%                  |       t        j&                  j)                  |||gd      S t        t*        t         t        t        | | t,        z         }t        j&                  j)                  |||gd      S )zMoving finger driven by the horn. FINGER_STYLE='finray' -> compliant TPU triangle (front/contact edge
    at Z=+GRIP_GAP/2 facing -Z, spine up); 'box' -> rigid v1. Plus a clamp bridging the base to the horn (+Y).r   r/   r0      r1   r   r   r"   )r   r   r   r"   r2   r3   )GRIP_GAPrC   r   r6   FINGER_STYLErV   
FINGER_LENFR_HFR_WALLFR_RIBSr   r   extrude_polygonFY1r'   arrayROOT_Xfloatr$   r=   unionTIP_XFT)zfr?   gussetproffrMfingers          r   build_thumbrn   S   s     
CBME#sCq"b1Fxz4'B--dC#I>HHr1a(1aC.1aB-VX]^
1$$b%%8$LL%c2rBw7F??  &%!8 LLr   c                     t          dz  } t        t        dt        t        | t
        z
  |       }t        ddt        t        d|       }t        dddt        dd      }t        j                  j                  |||gd	
      }t        dk(  rt        j                  j                  t        dz   t        t        g      }|j                  t        j                  j!                  t"        j$                  dz  g d             |j'                  t        dz
  dz  t        t        z   dz  | g       t        j                  j)                  ||gd	
      }|S )zLower finger (contact face at -GRIP_GAP/2, facing +Z) + an L-bracket routed UNDER the servo body
    (Z < body min -12.4) back to the holder so it doesn't intersect the servo.r   iiir0      iir2   r3   r   
   r   rX   r!   (   )rZ   r   rf   r6   ra   rg   r   r=   re   	PAD_STYLEr   r   r\   GVr$   r%   r&   r'   r(   r   r>   )rh   rm   strutbackjawgvs         r   build_fixedry   c   s    SB%c3R4Fc3S"-ERS#s+D
//

 5j

ICI!!:?B*C!D
722BB25519iXY
urzS0392CRHIoo((#r:(FJr   c                 J   t        j                  | d      }t        j                  |d|dd       t        j                  t        j                  |d      d|dd       t        j                  t        j                  |d	d
|t        t        |            z   i      dd|i       y )Nvisualorigin0 0 0xyzrpygeometrymeshz0.001 0.001 0.001)filenamescalematerialnamecolorrgba)ET
SubElementstrid)linkfncoloffvs        r   _visr   s   s}    
dH%AMM!Xs7;<MM"--:.RRe8fgMM"--:SAZ/HI7U[]`Tabr   c           	         t        j                  dddi      }t        j                  |dddi       t        j                  |dddd	      }t        j                  |d
ddi       t        j                  |dddi       t        j                  |dddd       t        j                  |dddi      }t        |dd       t        |dd       t        j                  |dddd	      }t        j                  |d
ddi       t        j                  |dddi       t        j                  |dt        dz  dddd       t        j                  |dddi       t        j                  |ddt
         d d d!       t        j                  |dddi      }t        |d"d#t         dz  dd       t        j                  |      j                  |        t        d$|        y )%Nrobotr   gripper_testingr   worldjointworld_to_basefixed)r   typeparentchildbaser|   r}   z
1.5708 0 0r~   z
st3215.stlz0.35 0.35 0.40 1gripper_base.stlz0.20 0.52 0.85 1rw   revolutethumb  z.5fz 0 0axisr   z0 1 0limitz-0.13)loweruppereffortvelocity	thumb.stlz0.90 0.65 0.20 1wrote)	r   Elementr   r   r:   
OPEN_ANGLEElementTreewriteprint)pathr   wjr   jjr   s         r   gen_urdfr   z   s   JJw): ;<EMM%&'!23	ug'Q	RBMM"h 12MM"g/0MM"h EF==(89D|/0!#56	ugz'J	KBMM"h 01MM"g01MM"h2d73-t(<W MNMM"fug./MM"gZLUXfijkMM%&'):;E/RCHS>1FGNN5%	'4r   __main__r   zfixed_jaw.stlzholder_approx_drilled.stlr   )forcer2   r3   r   F)only_watertightzthumb: bbox r"   z wt=z  fixed: bbox zgripper_base: vol=r   z.1fzcm3 wt=z bodies=z  (WARNING disjoint) )r}   )zgripper_testing.urdf)8__doc__numpyr'   r   xml.etree.ElementTreeetreer   r   r:   r;   r5   IDLER_YrZ   r\   r6   ra   rg   rs   rt   rc   rf   r   r[   r]   r^   r_   PTr7   r8   r<   r   r-   rC   rV   rn   ry   r   r   __name__tfexportloadholderr=   re   r   lensplitnbr   roundr   tolistis_watertightvolume r   r   <module>r      s  
   " " 
B 
S		c
Z
 

 	DJM  c, z=+-DAqHH[HH_W\\5VDF??  &!Z @DKK"#	TZZZ.	/B	L!))A.5578Q__<M N!!))A.5578: ;	t{{4/4GD<N<N;OxXZW[')Av#268 9J r   