
    <j%                     @   d Z ddlZddlmc mZ ddlmZ	 d\  Z
ZZdZ ej                  g d      Zg dZg dZg d	Zg d
Zd Z ee      D  c
g c]"  \  } \  }}}}}}}}||| e|e|          ||||f$ c
}}}}}}}}} ZddddZd Zd Zd Zd Zd Zd Zd#dZedk(  rhddlZdej@                  v Z!e!rdndZ" ej
                   ee!            Z# ejH                  e#d       e#jK                  e"dd        e&d e" d!e! d"       yyc c
}}}}}}}}} w )$u  SO-100 parametric rebuild from OUR locked parts (st3215 + wrap_holder + yoke_exact).

PHASE 1: reproduce the SO-ARM100 kinematic chain EXACTLY (joint origins/rpy/axes/limits from
Simulation/SO100/so100.urdf), but at each link place OUR servo+holder so the servo's real output
axis (+Y through (-0.0255,0,0) m) coincides with the joint that link drives. This is physically
correct (output shaft IS the joint) and needs no mesh registration — only the joint axis, which is
identical between their sts3215 and our st3215. Now 6 DOF + gripper (a parametric wrist_yaw was added on
top of the SO-100's 5) = 7 servos. Connectors come in Phase 2.

Each link i carries the servo for its OUTGOING joint i; the body straddles the joint and extends back
into the link. ROLLS[i] is the free roll about each output axis (optimized by so100_optimize_rolls.py to
aim each yoke at the next servo). The chain is fully parametric: add/remove a JOINTS+LINKS entry (and a
ROLLS/SPACERS slot) and the whole arm regenerates.
    N)Rotation)z
st3215.stlzwrap_holder.stlzyoke_exact.stlz0.001 0.001 0.001)g&1        r   ))shoulder_panbaseshoulder)r   gMSt$gL7A`?5$!?r   r   r      r      )shoulder_liftr   	upper_arm)r   g=
ףp=?gK=U?)r   r   r   r   r   r   g      @)
elbow_flexr   	lower_arm)r   g4cѼ?gy&1?r   r   5$!	r   )
wrist_flexr   wrist)r   g_Lu?g}8gD?)r   r   r   g      g333333?)
wrist_rollr   wrist2)r   gڬ\mŮr   )r   r	   r   r
   r   5$!	@)	wrist_yawr   gripper)r   g
ףp=
r   r   r
   r   g?)r   r   jaw)gMOgHPr   )r   r   r   )r   r   r   gɿg       @)r   r   r   r   r   r   r   r   )gz6>W?g	_@g@gMJ?g#J{/@goT?gT㥛Ġ@)r   r   g)\(?g{Gz?g?gQ?gQ?c                     t        j                  | t              }t         j                  j	                  |      }|dkD  rt        |d||z  z   z        S t        |      S )Ng&.>r   )nparrayfloatlinalgnormtuple)xyzsxns       5/data/cameron/repos/smith300_para_stuff/so100_para.py_spacedr*   ,   sJ    
eA
		qA%&X5a!a%i!;58;    z0.15 0.15 0.17 1z0.20 0.52 0.85 10.85 0.60 0.20 1)servoholderyokec                     t        j                  d      }t        j                  d|      j	                         |d dd df<   | |d ddf<   |S )N   r%      )r   eyeRot
from_euler	as_matrix)r%   rpyMs      r)   _Tr9   8   sN    
q	Auc*446Abqb"1"fIAbqb!eHHr+   c                 t    t        j                  d      }| j                         |d dd df<   ||d ddf<   |S )Nr1   r2   )r   r3   r6   )Rtr8   s      r)   _matr=   ?   s@    
q	AAbqb"1"fIAbqb!eHHr+   c                    t        j                  d|      j                         t        j                  |t
              z  }|t        j                  j                  |      z  }t        j                  |gg dg      \  }}|t        j                  d|      z  }t        j                  | t
              |j                  t              z
  }||fS )zcR_s, t_s placing our servo so +Y output -> joint axis (in parent frame), output pt at joint origin.r%   r
   y)r4   r5   r6   r   r    r!   r"   r#   align_vectorsapplyP0)	r%   r7   axisrolldR_align_R_st_ss	            r)   
servo_poserJ   F   s    uc",,.$1FFA	BIINN1A""A34JGQ
CNN3-
-C
((3
2
.C8Or+   c                    t        j                  | d      }|j                  d      \  }}}t        j                  |d|d   dd|d   dd|d   d|dd|dd|dd	       t        j                  t        j                  |d
      d|t        d       t        j                  t        j                  |dd|t	        t        |            z   i      ddt        |   i       y )Nvisualr%   originr   .6f r   r   r%   r7   geometrymeshfilenamescalematerialnamecolorrgba)ET
SubElementas_eulerSCALEstridCOLORS)	linkfnamecnamerH   rI   vrpr?   s	            r)   _meshrg   P   s    
dH%All5!GAq!MM!X#a&Qs1vcl!CF3<'P\]^a[bbcdeficjjklmnqkrYstuMM"--:.UUZ8[\MM"--:BqE
8J/KLgX^`fgl`mWnor+   c                    t        j                  | d      }t        j                  |d|dd       t        j                  t        j                  |d      dd|i       t        j                  t        j                  |dd	|i      d
d|i       y )NrL   rM   0 0 0rP   rQ   boxsizerV   rW   rX   rY   )rZ   r[   )ra   r%   rk   rY   rW   rd   s         r)   _boxrl   X   sl    
dH%AMM!Xs7;<MM"--:.~FMM"--:~>&RVXr+   c                    t        j                  |t              t        j                  |t              }}||z
  }t         j                  j	                  |      }|dk  ryt        j                  ||z  gg dg      \  }}	||z   dz  }
|j                  d      \  }}}t        j                  | d      }t        j                  |d|
d   d	d
|
d   d	d
|
d   d	|d	d
|d	d
|d	d       t        j                  t        j                  |d      dd|d	d
| d
| i       t        j                  t        j                  |dd|i      dd|i       y)z;Thick structural box spanning A->B (both in link frame, m).gư>Nr   r   r%   rL   rM   r   rN   rO   r   rP   rQ   rj   rk   rV   rW   rX   rY   )
r   r    r!   r"   r#   r4   r@   r\   rZ   r[   )ra   ABthickrY   rW   uLr;   rG   cre   rf   r?   rd   s                  r)   _barrt   _   sK   88Aurxx51qA	AA
		qA4xa!eWyk2DAq	
Q!AjjGAq!
dH%AMM!X!A$s1QqT#Ja!Sz'JVWX[U\\]^_`c]ddefghkelSmnoMM"--:.1S'5'QRSXRY@Z7[\MM"--:~>&RVXr+   c                    t        j                  dd| rdn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        t              D ]  \  }}t        j                  |dd|i      }|t        t              k  rPt        |   \  }}}}}}	}
}t        |||	t        |         \  }}t        |t        d||       t        |t        d||       |dk(  rt        |dddd       |dk(  rt        |dddd       |dk(  rt        |dddd       | sd |cxk  rt        t              k  sn t        |d!z
     }t        |d"   |d#   |d$   t        |d!z
           \  }}t        j                  j                  t!        |d"   |d#               t#        ||      z  }t%        j&                  |d d"d d"f         j)                  d%      \  }}}t        j                  |d&      }t        j                  |d|d'   d(d)|d*   d(d)|d+   d(|d(d)|d(d)|d(d       t        j                  t        j                  |d,      d-t*        t,        d.       t        j                  t        j                  |d/dd0|z   i      d1d2di       t        j                  |d&      }t        j                  |dddd       t        j                  t        j                  |d,      d-d3| d4t,        d.       t        j                  t        j                  |d/dd5|z   i      d1d2di        t        D ]  \  }}}}}}	}
}t        j                  |d|d6d
      }t        j                  |dd|i       t        j                  |dd|i       t        j                  |d|d     d)|d!    d)|d7    |d     d)|d!    d)|d7    d       t        j                  |d8d%|	d     d)|	d!    d)|	d7    i       t        j                  |d9t/        |
      t/        |      d:d;d<        |S )=NrobotrW   so100_para_connected
so100_parara   worldjointbase_fixfixed)rW   typeparentchildr   rM   ri   rP   r-   r.   z0 -0.0452 -0.01z0.08 0.08 0.006z0.90 0.75 0.15 1	baseplater   z0.0135 -0.055 0z0.012 0.05 0.02z0.80 0.55 0.18 1	fixed_jawr   z	0 -0.03 0r,   
moving_jawr   r   r2   r1      r%   rL   )r   r2   rN   rO   )r   r2   )r   r2   rQ   rR   rS   rV   ykrX   rY   so100_bridge_z.stlbrrevoluter   rC   limit351)loweruppereffortvelocity)rZ   Elementr[   	enumerateLINKSlenJOINTSrJ   ROLLSrg   SERVOHOLDERrl   r   r"   invr9   r=   r4   from_matrixr\   YOKEr]   r^   )connectrv   bjirW   ra   rG   r%   r7   rC   lohirH   rI   pjRptpPyrypywrd   vbjnamer~   r   js                              r)   genr   o   sW   JJw7)?P\ ]^EMM%&'!23	ug
G'L	MBMM"h 12MM"g/0MM"h @AU#4}}UFVTN;s6{?.4Qi+Aq!S#tR!#sD%(;HC$wS1$#s36>(*;=OQ\]9(*;=OQ\]5={$57I<X
 q1+F+AB1r!ubeU1q5\BFB		bA1./$r2,>A2A2rr63<<UCJBBdH-AMM!X!C&Qqvcl!AcFSV</X24S2c(!Bs8/L(N OMM"--:6T\a@bcMM"--:t7LM!F,>#?A
 tX.BMM"h(HIMM"--J7)6tfD'AERTMM"--J8MN!F,>#?AG $L 9?4vuc3b"MM%5**MN
aFF#34
a65/2
ac!fXQs1vhaAx+HTWXYTZS[[\]`ab]c\ddefijkflemQn#op
a%DG9Ad1gYaQy)I!JK
aCGc"gQUcf"gh 9? Lr+   __main__r   zso100_para_connected.urdfzso100_para.urdf)r   z  unicodeT)encodingxml_declarationzwrote z8 (SO-100 chain, our servo+holder, 5DOF+gripper, connect=))F)'__doc__numpyr   xml.etree.ElementTreeetreeElementTreerZ   scipy.spatial.transformr   r4   r   r   r   r]   r    rB   r   r   r   SPACERSr*   r   r`   r9   r=   rJ   rg   rl   rt   r   __name__sysargvr   outtreeindentwriteprint)	r   r(   rf   rs   r%   r7   axr   r   s	   000000000r)   <module>r      s~    " " 3G vtRXX!"
 	\? 0< 5>f4E
G 
G4E0a0!Q3RR aGC,c2r2
>4E
G 
G &1CM_	`pYY 5p z388#G)0
%6GC2>>#g./DBIIdDJJsYJ=	F3%OPWyXY
Z[ k
G 
Gs   'D