Coverage for trimesh/poses.py: 95%
126 statements
« prev ^ index » next coverage.py v7.14.1, created at 2026-07-31 23:55 +0000
1"""
2poses.py
3-----------
5Find stable orientations of meshes.
6"""
8import numpy as np
10from .triangles import points_to_barycentric
11from .typed import Seed
12from .util import diagonal_dot, random_generator
14try:
15 import networkx as nx
16except BaseException as E:
17 # create a dummy module which will raise the ImportError
18 # or other exception only when someone tries to use networkx
19 from .exceptions import ExceptionWrapper
21 nx = ExceptionWrapper(E)
24def compute_stable_poses(
25 mesh, center_mass=None, sigma=0.0, n_samples=1, threshold=0.0, seed: Seed = None
26):
27 """
28 Computes stable orientations of a mesh and their quasi-static probabilities.
30 This method samples the location of the center of mass from a multivariate
31 gaussian with the mean at the center of mass, and a covariance
32 equal to and identity matrix times sigma, over n_samples.
34 For each sample, it computes the stable resting poses of the mesh on a
35 a planar workspace and evaluates the probabilities of landing in
36 each pose if the object is dropped onto the table randomly.
38 This method returns the 4x4 homogeneous transform matrices that place
39 the shape against the planar surface with the z-axis pointing upwards
40 and a list of the probabilities for each pose.
42 The transforms and probabilities that are returned are sorted, with the
43 most probable pose first.
45 Parameters
46 ----------
47 mesh : trimesh.Trimesh
48 The target mesh
49 com : (3,) float
50 Rhe object center of mass. If None, this method
51 assumes uniform density and watertightness and
52 computes a center of mass explicitly
53 sigma : float
54 Rhe covariance for the multivariate gaussian used
55 to sample center of mass locations
56 n_samples : int
57 The number of samples of the center of mass location
58 threshold : float
59 The probability value at which to threshold
60 returned stable poses
62 seed : None or int
63 Seed for deterministic results, otherwise OS entropy.
65 Returns
66 -------
67 transforms : (n, 4, 4) float
68 The homogeneous matrices that transform the
69 object to rest in a stable pose, with the
70 new z-axis pointing upwards from the table
71 and the object just touching the table.
72 probs : (n,) float
73 Probability in (0, 1) for each pose
74 """
76 # save convex hull mesh to avoid a cache check
77 cvh = mesh.convex_hull
79 if center_mass is None:
80 center_mass = mesh.center_mass
82 # Sample center of mass, rejecting points outside of conv hull
83 random = random_generator(seed)
84 sample_coms = []
85 while len(sample_coms) < n_samples:
86 remaining = n_samples - len(sample_coms)
87 coms = random.multivariate_normal(center_mass, sigma * np.eye(3), remaining)
88 for c in coms:
89 dots = diagonal_dot(c - cvh.triangles_center, cvh.face_normals)
90 if np.all(dots < 0):
91 sample_coms.append(c)
93 norms_to_probs = {} # Map from normal to probabilities
95 # For each sample, compute the stable poses
96 for sample_com in sample_coms:
97 # Create toppling digraph
98 dg = _create_topple_graph(cvh, sample_com)
100 # Propagate probabilities to sink nodes with a breadth-first traversal
101 nodes = [n for n in dg.nodes() if dg.in_degree(n) == 0]
102 n_iters = 0
103 while len(nodes) > 0 and n_iters <= len(mesh.faces):
104 new_nodes = []
105 for node in nodes:
106 if dg.out_degree(node) == 0:
107 continue
108 successor = next(iter(dg.successors(node)))
109 dg.nodes[successor]["prob"] += dg.nodes[node]["prob"]
110 dg.nodes[node]["prob"] = 0.0
111 new_nodes.append(successor)
112 nodes = new_nodes
113 n_iters += 1
115 # Collect stable poses
116 for node in dg.nodes():
117 if dg.nodes[node]["prob"] > 0.0:
118 normal = cvh.face_normals[node]
119 prob = dg.nodes[node]["prob"]
120 key = tuple(np.around(normal, decimals=3))
121 if key in norms_to_probs:
122 norms_to_probs[key]["prob"] += 1.0 / n_samples * prob
123 else:
124 norms_to_probs[key] = {
125 "prob": 1.0 / n_samples * prob,
126 "normal": normal,
127 }
129 transforms = []
130 probs = []
132 # Filter stable poses
133 for key in norms_to_probs:
134 prob = norms_to_probs[key]["prob"]
135 if prob > threshold:
136 tf = np.eye(4)
138 # Compute a rotation matrix for this stable pose
139 z = -1.0 * norms_to_probs[key]["normal"]
140 x = np.array([-z[1], z[0], 0])
141 if np.linalg.norm(x) == 0.0:
142 x = np.array([1, 0, 0])
143 else:
144 x = x / np.linalg.norm(x)
145 y = np.cross(z, x)
146 y = y / np.linalg.norm(y)
147 tf[:3, :3] = np.array([x, y, z])
149 # Compute the necessary translation for this stable pose
150 m = cvh.copy()
151 m.apply_transform(tf)
152 z = -m.bounds[0][2]
153 tf[:3, 3] = np.array([0, 0, z])
155 transforms.append(tf)
156 probs.append(prob)
158 # Sort the results
159 transforms = np.array(transforms)
160 probs = np.array(probs)
161 inds = np.argsort(-probs)
163 return transforms[inds], probs[inds]
166def _orient3dfast(plane, pd):
167 """
168 Performs a fast 3D orientation test.
170 Parameters
171 ----------
172 plane: (3,3) float, three points in space that define a plane
173 pd: (3,) float, a single point
175 Returns
176 -------
177 result: float, if greater than zero then pd is above the plane through
178 the given three points, if less than zero then pd is below
179 the given plane, and if equal to zero then pd is on the
180 given plane.
181 """
182 pa, pb, pc = plane
183 adx = pa[0] - pd[0]
184 bdx = pb[0] - pd[0]
185 cdx = pc[0] - pd[0]
186 ady = pa[1] - pd[1]
187 bdy = pb[1] - pd[1]
188 cdy = pc[1] - pd[1]
189 adz = pa[2] - pd[2]
190 bdz = pb[2] - pd[2]
191 cdz = pc[2] - pd[2]
193 return (
194 adx * (bdy * cdz - bdz * cdy)
195 + bdx * (cdy * adz - cdz * ady)
196 + cdx * (ady * bdz - adz * bdy)
197 )
200def _compute_static_prob(tri, com):
201 """
202 For an object with the given center of mass, compute
203 the probability that the given tri would be the first to hit the
204 ground if the object were dropped with a pose chosen uniformly at random.
206 Parameters
207 ----------
208 tri: (3,3) float, the vertices of a triangle
209 cm: (3,) float, the center of mass of the object
211 Returns
212 -------
213 prob: float, the probability in [0,1] for the given triangle
214 """
215 sv = [(v - com) / np.linalg.norm(v - com) for v in tri]
217 # Use L'Huilier's Formula to compute spherical area
218 a = np.arccos(min(1, max(-1, np.dot(sv[0], sv[1]))))
219 b = np.arccos(min(1, max(-1, np.dot(sv[1], sv[2]))))
220 c = np.arccos(min(1, max(-1, np.dot(sv[2], sv[0]))))
221 s = (a + b + c) / 2.0
223 # Prevents weirdness with arctan
224 try:
225 return (
226 1.0
227 / np.pi
228 * np.arctan(
229 np.sqrt(
230 np.tan(s / 2)
231 * np.tan((s - a) / 2)
232 * np.tan((s - b) / 2)
233 * np.tan((s - c) / 2)
234 )
235 )
236 )
237 except BaseException:
238 s = s + 1e-8
239 return (
240 1.0
241 / np.pi
242 * np.arctan(
243 np.sqrt(
244 np.tan(s / 2)
245 * np.tan((s - a) / 2)
246 * np.tan((s - b) / 2)
247 * np.tan((s - c) / 2)
248 )
249 )
250 )
253def _create_topple_graph(cvh_mesh, com):
254 """
255 Constructs a toppling digraph for the given convex hull mesh and
256 center of mass.
258 Each node n_i in the digraph corresponds to a face f_i of the mesh and is
259 labelled with the probability that the mesh will land on f_i if dropped
260 randomly. Not all faces are stable, and node n_i has a directed edge to
261 node n_j if the object will quasi-statically topple from f_i to f_j if it
262 lands on f_i initially.
264 This computation is described in detail in
265 http://goldberg.berkeley.edu/pubs/eps.pdf.
267 Parameters
268 ----------
269 cvh_mesh : trimesh.Trimesh
270 Rhe convex hull of the target shape
271 com : (3,) float
272 The 3D location of the target shape's center of mass
274 Returns
275 -------
276 graph : networkx.DiGraph
277 Graph representing static probabilities and toppling
278 order for the convex hull
279 """
280 adj_graph = nx.Graph()
281 topple_graph = nx.DiGraph()
283 # Create face adjacency graph
284 face_pairs = cvh_mesh.face_adjacency
285 edges = cvh_mesh.face_adjacency_edges
287 graph_edges = []
288 for fp, e in zip(face_pairs, edges):
289 verts = cvh_mesh.vertices[e]
290 graph_edges.append([fp[0], fp[1], {"verts": verts}])
292 adj_graph.add_edges_from(graph_edges)
294 # Compute static probabilities of landing on each face
295 for i, tri in enumerate(cvh_mesh.triangles):
296 prob = _compute_static_prob(tri, com)
297 topple_graph.add_node(i, prob=prob)
299 # Compute COM projections onto planes of each triangle in cvh_mesh
300 proj_dists = diagonal_dot(cvh_mesh.face_normals, com - cvh_mesh.triangles[:, 0])
301 proj_coms = com - proj_dists[:, None] * cvh_mesh.face_normals
302 barys = points_to_barycentric(cvh_mesh.triangles, proj_coms)
303 unstable_face_indices = np.where(np.any(barys < 0, axis=1))[0]
305 # For each unstable face, compute the face it topples to
306 for fi in unstable_face_indices:
307 proj_com = proj_coms[fi]
308 centroid = cvh_mesh.triangles_center[fi]
309 norm = cvh_mesh.face_normals[fi]
311 for tfi in adj_graph[fi]:
312 v1, v2 = adj_graph[fi][tfi]["verts"]
313 if np.dot(np.cross(v1 - centroid, v2 - centroid), norm) < 0:
314 tmp = v2
315 v2 = v1
316 v1 = tmp
317 plane1 = [centroid, v1, v1 + norm]
318 plane2 = [centroid, v2 + norm, v2]
319 if (
320 _orient3dfast(plane1, proj_com) >= 0
321 and _orient3dfast(plane2, proj_com) >= 0
322 ):
323 break
325 topple_graph.add_edge(fi, tfi)
327 return topple_graph