Mi (SrSSKrSSKJr SSKJr SSKrS Sjr SrS rS rg!\ ar SSK J r \ "\ 5rSr C N(Sr C ff=f) z; poses.py ----------- Find stable orientations of meshes. N)points_to_barycentric) diagonal_dot)ExceptionWrapperc * URnUc URn/n[U5U:aU[U5- n[RR X[R "S5-U5nUHVn [XR- UR5n [R"U S:5(dMEURU 5 MX [U5U:aM0n UGHn [X\5n U R5Vs/sHoRU5S:XdMUPM nnSn[U5S:aU[UR5::a/nUHnU R!U5S:XaM[#[%U R'U555nU RUS==U RUS- ss'SU RUS'URU5 M UnUS- n[U5S:aU[UR5::aMU R5HnU RUSS:dMURUnU RUSn[)[R*"USS95nUU ;aU US==SU- U-- ss'MxSU- U-US .U U'M GM /n/nU GHnU USnUU:dM[R "S 5nS U US -n[R,"US*USS/5n[R.R1U5S:Xa[R,"/S Q5nO"U[R.R1U5- n[R2"UU5nU[R.R1U5- n[R,"UUU/5USS2SS24'UR55nUR7U5 UR8SS*n[R,"SSU/5USS2S4'URU5 URU5 GM [R,"U5n[R,"U5n[R:"U*5nUUUU4$s snf)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 Nrprobr)decimals?)r normalgr )rrr) convex_hull center_masslennprandommultivariate_normaleyertriangles_center face_normalsallappend_create_topple_graphnodes in_degreefaces out_degreenextiter successorstuplearoundarraylinalgnormcrosscopyapply_transformboundsargsort)meshrsigma n_samples thresholdcvh sample_coms remainingcomscdotsnorms_to_probs sample_comdgnrn_iters new_nodesnode successorr r key transformsprobstfzxymindss g/var/www/eduai.edurigo.com/storigo/production/storigo_env/lib/python3.13/site-packages/trimesh/poses.pycompute_stable_posesrIs`   C&& K k Y &K 00 yy,,["&&):KYWA$8$8 8#:J:JKDvvdQh""1% k Y &N" !# 2HHJ?Jq,,q/Q*>J?%j1nC O!;I==&!+ bmmD&9!:; #F+rxx~f/EE+),v&  + E qLG%j1nC O!;HHJDxx~f%+))$/xx~f-BIIfq9:.("3'/3?T3II/!$i$ 6"(+N3')"BJ Ec"6* ) B~c*844A1Q4%1q)*Ayy~~a C'HHY' q))AABIINN1%%A1a),Brr2A2vJ A  b !!QA!Q+Brr1uI   b ! LL /4*%J HHUOE ::uf D d U4[ ((}@s R RcUup#nUSUS- nUSUS- nUSUS- nUSUS- nUSUS- n USUS- n USUS- n USUS- n USUS- n XYU -X-- -XjU -X-- --XxU -X-- --$)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 rH _orient3dfastrZs 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 Sy39$% sSY& ' ( sSY& ' (c UVs/sH)o"U- [RRX!- 5- PM+ nn[R"[ S[ S[R "USUS5555n[R"[ S[ S[R "USUS5555n[R"[ S[ S[R "USUS5555nXE-U-S- nS[R- [R"[R"[R"US- 5[R"Xt- S- 5-[R"Xu- S- 5-[R"Xv- S- 5-55-$s snf![a US-nS[R- [R"[R"[R"US- 5[R"Xt- S- 5-[R"Xu- S- 5-[R"Xv- S- 5-55-s$f=f)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 rrrg@r g:0yE>) rr&r'arccosminmaxdotpiarctansqrttan BaseException)tricomvsvabr5ss rH_compute_static_probrns8; ;s!s7biinnQW- -sB ; #aR1r!u!5678A #aR1r!u!5678A #aR1r!u!5678A cA ee iiFF1q5Mffaeq[)*ffaeq[)*ffaeq[)*   <,   H ee iiFF1q5Mffaeq[)*ffaeq[)*ffaeq[)*    s0F:B(F??B8I:9I:c :[R"5n[R"5nURnURn/n[ XE5H0upxUR Un URUSUSSU 0/5 M2 URU5 [UR5Hup[X5n URXS9 M! [URXRSS2S4- 5n XSS2S4UR-- n[URU5n[ R""[ R$"US:SS95SnUHnUUnUR&UnURUnUUHnUUUSunn[ R("[ R*"UU- UU- 5U5S:aUnUnUnUUUU-/nUUU-U/n[-UU5S:dMo[-UU5S:dM O UR/UW5 M U$)a 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)r N)axis)nxGraphDiGraphface_adjacencyface_adjacency_edgeszipverticesradd_edges_from enumerate trianglesrnadd_noderrrrwhereanyrrar(rZadd_edge)cvh_meshrh adj_graph topple_graph face_pairsedges graph_edgesfperpirgr proj_dists proj_comsbarysunstable_face_indicesfiproj_comcentroidr'tfiv1v2tmpplane1plane2s rHrrs 6 I::?B$R=,,R0$$R(R=Cr]3'0FBvvbhhrH}b8mrsX,E)PD2 jJO- ! B s'AAA