geometry, so MuJoCo reports no contacts. This parses the URDF, does forward kinematics at a set of joint poses, places every visual mesh in the world, and runs true mesh-mesh collision (trimesh CollisionManager / FCL). Reports colliding part pairs, flagging SERVO<->SERVO overlaps (always unintended) separately from connector<->holder (intended mates). Usage: so100_check.py [q] (q applied to all revolute joints; default 0) N)Rotationz(/data/cameron/repos/smith300_para_stuff/ctjd}tjd|j |ddddf<||dddf<|S)Nxyz)npeyeRot from_euler as_matrix)rrpyMs 6/data/cameron/repos/smith300_para_stuff/so100_check.pyTrsN q Auc*446Abqb"1"fIAbqb!eH Hc fg}|jdD]=}|jd}|jdjDcgc] }t |}}|jdjDcgc] }t |}}|jd}|6|jdjDcgc] }t |c}ngd} |j |jd|jdjd |jd jd ||| |jd f@|D chc] \} } } } } } } | } } } |jd D cgc]'} | jd| vs| jd)}} |Dcic]}|t jd }}d }|rd}|D]\}}} }}} }||vs| |vs||t||z}|dk(rt|tr|j|dn|}t jd }tjt j| |zj|ddddf<||z}||| <d }|r|Scc}wcc}wcc}wcc} } wcc} wcc}w)Njointoriginrr axisrrrnameparentlinkchildtyperTFrevolutegr)findallfindgetsplitfloatappendrr r isinstancedictr from_rotvecarrayr )rootqjointsjoxrr rax_cchildrenlrootsrworldchangedjnpjtrangRs rfk_worldr;su F \\' " FF8 !"u!3!3!56!5AuQx!56!"u!3!3!56!5AuQx!56vvf~<@?maeeEl&8&8&:QZ&Z\&Z58&ZC\>?maeeEl&8&8&:QZ&Z\&Z58&ZC\66*%D99V$dii&6qA}()gw(?(E(E(GH(G1eAh(GHLLQUU:%6!6fEJJL b!uuZ(..s3B7==cB1E)*v)<)<)>?)>AuQx)>?$$(((5ffZ(*-/swwvRDz  eDkAc3K7 8 NNdV1TF+Q /'4 "0***=JC&+Oedax1}QAq6eKO',butq!x1}aAGGTWLYZO9[ AI(a-AGGCLOqwwWZ|\]<^FuLb  $$1]\I @Pbs1NN ?N8N0 N=NNA0N!c ttjdkDrtjdnd}ttjdkDrtjdnd}d|vr[|jdDcic]@}|jddj t |jddBc}n t |}t ||\}}td|d t|d t|t|D]\}}td |d |t|D]\}}td |d ||Scc}w)Nzso100_para_connected.urdfz0.0=,rzq=z: SERVO-SERVO=z BRIDGE-thru-OTHER-SERVO=z SERVO-SERVO z <-> z BRIDGE-SERVO ) lensysargvr stripr!rnprintsorted)r]argkvr(rlrmrkrcs rmainr|]s.chh-!+388A;1LDSXX*#((1+CX[_bXbSYYs^T^r#q    !5#q)9#: :^ThmnqhrA %dAK BqcK 011KCP\L]K^ _`{#1 s'!-.$|$1 s'!-.%  Us:AE __main__)__doc__runumpyrrRxml.etree.ElementTreeetree ElementTreerNscipy.spatial.transformrr rPrr;rnr|__name__rrrsL ""3. :#%L  zFr