l0j(dZddlZddlmZ ddlZn##e$rZddl m Z e eZYdZ[ndZ[wwxYwd dZ dZ d Z d ZdS) z; poses.py ----------- Find stable orientations of meshes. N)points_to_barycentric)ExceptionWrapperc < |j}||j}g}t||kr|t|z }tj||tjdz|}|D]R} tjd| |jz |j } tj | dkr| | St||ki} |D]} t|| fd D} d}t| dkr|t|jkrg}| D]}|dkrt!t#|}j |dxxj |dz cc<dj |d<| ||} |dz }t| dkr|t|jkӉ D]}j |ddkrr|j |}j |d}t'tj|d }|| vr| |dxxd |z |zz cc<}d |z |z|d | |<g}g}| D]}| |d}||krtjd }d | |dz}tj|d |ddg}tj|dkrtjgd}n"|tj|z }tj||}|tj|z }tj|||g|ddddf<|}|||jdd }tjdd|g|dddf<| || |tj|}tj|}tj| }||||fS)a Computes stable orientations of a mesh and their quasi-static probabilities. This method samples the location of the center of mass from a multivariate gaussian with the mean at the center of mass, and a covariance equal to and identity matrix times sigma, over n_samples. For each sample, it computes the stable resting poses of the mesh on a a planar workspace and evaluates the probabilities of landing in each pose if the object is dropped onto the table randomly. This method returns the 4x4 homogeneous transform matrices that place the shape against the planar surface with the z-axis pointing upwards and a list of the probabilities for each pose. The transforms and probabilities that are returned are sorted, with the most probable pose first. Parameters ---------- mesh : trimesh.Trimesh The target mesh com : (3,) float Rhe object center of mass. If None, this method assumes uniform density and watertightness and computes a center of mass explicitly sigma : float Rhe covariance for the multivariate gaussian used to sample center of mass locations n_samples : int The number of samples of the center of mass location threshold : float The probability value at which to threshold returned stable poses Returns ------- transforms : (n, 4, 4) float The homogeneous matrices that transform the object to rest in a stable pose, with the new z-axis pointing upwards from the table and the object just touching the table. probs : (n,) float Probability in (0, 1) for each pose Nij,ij->ircFg|]}|dk|S)r) in_degree).0ndgs P/home/wildlama/miniconda3/envs/lam/lib/python3.11/site-packages/trimesh/poses.py z(compute_stable_poses..]s-???q",,q//Q*>*>*>*>*>probrr)decimals?)rnormalgr)rrr) convex_hull center_masslennprandommultivariate_normaleyeeinsumtriangles_center face_normalsallappend_create_topple_graphnodesfaces out_degreenextiter successorstuplearoundarraylinalgnormcrosscopyapply_transformboundsargsort)meshrsigma n_samples thresholdcvh sample_coms remainingcomscdotsnorms_to_probs sample_comr%n_iters new_nodesnode successorrrkey transformsprobstfzxymindsrs @rcompute_stable_posesrNs`  C& K k  Y & &K 0 00 y,,[%"&)):KYWW & &A9ZS-A)A3CSTTDvdQh &""1%%% k  Y & &N" !#z 2 2@???BHHJJ???%jj1nnC OO!;!;I , ,==&&!++ bmmD&9&9!:!:;; #F+++rx~f/EE+++),v&  ++++E qLG%jj1nnC OO!;!;HHJJ  Dx~f%++)$/x~f-BIfq999::.(("3'///3?T3II////!$i$ 6"(++N3' J Ec"6* )  B~c*844A1Q4%1q)**Ay~~a  C''HYYY'' q)))AABINN1%%%A1a),,Brr2A2vJ A  b ! ! !!QA!Q++Brr1uI   b ! ! ! LL   *%%J HUOOE :uf  D d U4[ ((rc|\}}}|d|dz }|d|dz }|d|dz }|d|dz }|d|dz } |d|dz } |d|dz } |d|dz } |d|dz } || | z| | zz z|| | z| |zz zz||| z| | zz zzS)a Performs a fast 3D orientation test. Parameters ---------- plane: (3,3) float, three points in space that define a plane pd: (3,) float, a single point Returns ------- result: float, if greater than zero then pd is above the plane through the given three points, if less than zero then pd is below the given plane, and if equal to zero then pd is on the given plane. rrr)planepdpapbpcadxbdxcdxadybdycdyadzbdzcdzs r _orient3dfastr_s JBB Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C Q%"Q%-C sSy39$% sS3Y& ' ( sS3Y& ' (rc fd|D}tjtdtdtj|d|d}tjtdtdtj|d|d}tjtdtdtj|d|d}||z|zdz } dtjz tjtjtj|dz tj||z dz ztj||z dz ztj||z dz zzS#t$r|dz}dtjz tjtjtj|dz tj||z dz ztj||z dz ztj||z dz zzcYSwxYw) a For an object with the given center of mass, compute the probability that the given tri would be the first to hit the ground if the object were dropped with a pose chosen uniformly at random. Parameters ---------- tri: (3,3) float, the vertices of a triangle cm: (3,) float, the center of mass of the object Returns ------- prob: float, the probability in [0,1] for the given triangle c`g|]*}|z tj|z z +SrP)rr.r/)r vcoms rrz(_compute_static_prob..s4 ; ; ;!1s7binnQW-- - ; ; ;rrrrg@rg:0yE>) rarccosminmaxdotpiarctansqrttan BaseException)trircsvabr=ss ` r_compute_static_probrss4 < ; ; ;s ; ; ;B #aR1r!u!5!5667788A #aR1r!u!5!5667788A #aR1r!u!5!5667788A QcA e iF1q5MMfa!eq[))*fa!eq[))*fa!eq[))*       H e iF1q5MMfa!eq[))*fa!eq[))*fa!eq[))*    s BF%%B+IIc vtj}tj}|j}|j}g}t ||D]8\}}|j|} ||d|dd| ig9||t|j D],\} } t| |} | | | -tjd|j||j dddfz } |tjd| |jz }t!|j |}tjtj|dkdd}|D]}||}|j|}|j|}||D]}|||d\}}tjtj||z ||z |dkr|}|}|}||||zg}|||z|g}t-||dkrt-||dkrn||||S) aW Constructs a toppling digraph for the given convex hull mesh and center of mass. Each node n_i in the digraph corresponds to a face f_i of the mesh and is labelled with the probability that the mesh will land on f_i if dropped randomly. Not all faces are stable, and node n_i has a directed edge to node n_j if the object will quasi-statically topple from f_i to f_j if it lands on f_i initially. This computation is described in detail in http://goldberg.berkeley.edu/pubs/eps.pdf. Parameters ---------- cvh_mesh : trimesh.Trimesh Rhe convex hull of the target shape com : (3,) float The 3D location of the target shape's center of mass Returns ------- graph : networkx.DiGraph Graph representing static probabilities and toppling order for the convex hull rrverts)rr Nzi,ij->ij)axis)nxGraphDiGraphface_adjacencyface_adjacency_edgeszipverticesr#add_edges_from enumerate trianglesrsadd_noderrr!rwhereanyr rhr0r_add_edge)cvh_meshrc adj_graph topple_graph face_pairsedges graph_edgesfperuirnr proj_dists proj_comsbarysunstable_face_indicesfiproj_comcentroidr/tfiv1v2tmpplane1plane2s rr$r$su6 I:<%>%>??B$''R=,R0$R(R=  Cr]3'0FBvbhrH}b8m<rs,,,,,,-,,,,,  !  BBBBBB E)E)E)E)PD2 2 2 jLLLLLs3.3