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