def merge_triangle_meshes(*items):
merged_geom = Geometry3D()
verts = []
tris = []
nverts = 0
for i, item in enumerate(items):
if isinstance(item, Geometry3D):
xform, (iverts, itris) = numpy_convert.to_numpy(item)
elif hasattr(item, 'geometry'):
xform, (iverts, itris) = numpy_convert.to_numpy(item.geometry())
else:
raise ValueError(
"Don't know how to merge trimesh from item of type " +
item.__class__.__name__)
verts.append(
np.dot(np.hstack((iverts, np.ones((len(iverts), 1)))),
xform.T)[:, :3])
tris.append(itris + nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts, tris), 'TriangleMesh')
merged_geom.setTriangleMesh(mesh)
return merged_geom
def __init__(self,
env_obj,
env_geom,
gravity_coefficient=-9.8,
gravity_direction=[0, 0, 1],
mass=1.0):
self.env_obj = env_obj
self.env_geom = env_geom
pointcloud = to_numpy(self.env_geom.pc.getPointCloud(),
type="PointCloud")
R = to_numpy(self.env_geom.pc.getCurrentTransform()[0], 'Matrix3')
t = to_numpy(self.env_geom.pc.getCurrentTransform()[1], 'Vector3')
pointcloud[:, :3] = np.dot(R, pointcloud.T[:3, :]).T + t
self.point_cloud_array = pointcloud[:, :3]
self.point_cloud_tree = KDTree(self.point_cloud_array, leaf_size=100)
self.gravity_coefficient = gravity_coefficient
self.gravity_direction = gravity_direction
self.mass = mass
def init(self, scene, object, hints):
"""Needs object to contain a structured PointCloud."""
if not isinstance(object, (RigidObjectModel, Geometry3D, PointCloud)):
print(
"Need to pass an object as a RigidObjectModel, Geometry3D, or PointCloud"
)
return False
if isinstance(object, RigidObjectModel):
return self.init(scene, object.geometry(), hints)
pc = None
xform = None
if isinstance(object, Geometry3D):
pc = object.getPointCloud()
xform = object.getCurrentTransform()
else:
pc = object
xform = se3.identity()
self.pc = pc
self.pc_xform = xform
#now look through PC and find flat parts
#do a spatial hash
from collections import defaultdict
estimation_knn = 6
pts = numpy_convert.to_numpy(pc)
N = pts.shape[0]
positions = pts[:, :3]
normals = np.zeros((N, 3))
indices = (positions * (1.0 / self._gripper.opening_span)).astype(int)
pt_hash = defaultdict(list)
for i, (ind, p) in enumerate(zip(indices, positions)):
pt_hash[ind].append((i, p))
options = []
for (ind, iplist) in pt_hash.items():
if len(iplist) < estimation_knn:
pass
else:
pindices = [ip[0] for ip in iplist]
pts = [ip[1] for ip in iplist]
c, n = fit_plane_centroid(pts)
if n[2] < 0:
n = vectorops.mul(n, -1)
verticality = self.vertical_penalty(math.acos(n[2]))
var = sum(
vectorops.dot(vectorops.sub(p, c), n)**2 for p in pts)
roughness = self.roughness_penalty(var)
options.append((cn, n, verticality + roughness))
if len(options) == 0:
return False
self.options = options.sorted(key=lambda x: -x[2])
self.index = 0
return True
def get_geometry(self,robot,qfinger=None,type='Group'):
"""Returns a Geometry of the gripper frozen at its configuration.
If qfinger = None, the current configuration is used. Otherwise,
qfinger is a finger configuration.
type can be 'Group' (most general and fastest) or 'TriangleMesh'
(compatible with Jupyter notebook visualizer.)
"""
if qfinger is not None:
q0 = robot.getConfig()
robot.setConfig(self.set_finger_config(q0,qfinger))
res = Geometry3D()
gripper_links = self.gripper_links if self.gripper_links is not None else [self.base_link] + self.descendant_links(robot)
if type == 'Group':
res.setGroup()
Tbase = robot.link(self.base_link).getTransform()
for i,link in enumerate(gripper_links):
Trel = se3.mul(se3.inv(Tbase),robot.link(link).getTransform())
g = robot.link(link).geometry().clone()
if not g.empty():
g.setCurrentTransform(*se3.identity())
g.transform(*Trel)
else:
print("Uh... link",robot.link(link).getName(),"has empty geometry?")
res.setElement(i,g)
if qfinger is not None:
robot.setConfig(q0)
return res
else:
import numpy as np
from klampt.io import numpy_convert
#merge the gripper parts into a static geometry
verts = []
tris = []
nverts = 0
for i,link in enumerate(gripper_links):
xform,(iverts,itris) = numpy_convert.to_numpy(robot.link(link).geometry())
verts.append(np.dot(np.hstack((iverts,np.ones((len(iverts),1)))),xform.T)[:,:3])
tris.append(itris+nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts,tris),'TriangleMesh')
res.setTriangleMesh(mesh)
if qfinger is not None:
robot.setConfig(q0)
return res
def get_object_normals(obj):
"""General-purpose object normal getter"""
if isinstance(obj, TriangleMesh):
verts, tris = numpy_convert.to_numpy(obj)
return get_triangle_normals(verts, tris)
elif isinstance(obj, PointCloud):
return geometry.point_cloud_normals(obj)
elif isinstance(obj, Geometry3D):
if obj.type() == 'TriangleMesh':
return get_object_normals(obj.getTriangleMesh())
elif obj.type() == 'PointCloud':
return get_object_normals(obj.getPointCloud())
else:
raise ValueError(
"Can only get normals for triangle mesh and point cloud")
elif hasattr(obj, 'geometry'):
return get_object_normals(obj.geometry())
else:
raise ValueError(
"Invalid object sent to get_object_normals, can only be a TriangleMesh or PointCloud"
)
def stable_faces(obj, com=None, stability_tol=0.0, merge_tol=0.0):
"""
Returns a list of support polygons on the object that are
likely to be stable on a planar surface.
Args:
obj (RigidObjectModel or Geometry3D): the object.
com (3-list, optional): sets the local center of mass. If
not given, the default RigidObjectModel's COM will be used,
or (0,0,0) will be used for a Geometry3D.
stability_tol (float,optional): if > 0, then only faces that
are stable with all perturbed "up" directions (dx,dy,1) with
||(dx,dy)||<= normal_tol will be outputted (robust stability).
If < 0, then all faces that are stable from some "up" direction
(dx,dy,1) with ||(dx,dy)||<= |normal_tol| will be outputted
(non-robust stability)
merge_tol (float, optional): if > 0, then adjacent faces with
normals whose angles are within this tolerance (in rads) will
be merged together.
Returns:
list of list of 3-vectors: The set of all polygons that could
form stable sides. Each polygon is convex and listed in
counterclockwise order (i.e., the outward normal can be obtained
via:
(poly[2]-poly[0]) x (poly[1]-poly[0])
"""
if isinstance(obj, RigidObjectModel):
geom = obj.geometry()
if com is None:
com = obj.getMass().getCom()
else:
geom = obj
if com is None:
com = (0, 0, 0)
assert len(com) == 3, "Need to provide a 3D COM"
ch_trimesh = geom.convert('ConvexHull').convert('TriangleMesh')
xform, (verts, tris) = numpy_convert.to_numpy(ch_trimesh)
trinormals = get_triangle_normals(verts, tris)
edges = dict()
tri_neighbors = np.full(tris.shape, -1, dtype=np.int32)
for ti, tri in enumerate(tris):
for ei, e in enumerate([(tri[0], tri[1]), (tri[1], tri[2]),
(tri[2], tri[0])]):
if (e[1], e[0]) in edges:
tn, tne = edges[(e[1], e[0])]
if tri_neighbors[tn][tne] >= 0:
print("Warning, triangle", ti,
"neighbors two triangles on edge", tne, "?")
tri_neighbors[ti][ei] = tn
tri_neighbors[tn][tne] = ti
else:
edges[e] = ti, ei
num_empty_edges = 0
for ti, tri in enumerate(tris):
for e in range(3):
if tri_neighbors[tn][e] < 0:
num_empty_edges += 1
if num_empty_edges > 0:
print("Info: boundary of mesh has", num_empty_edges, "edges")
visited = [False] * len(tris)
cohesive_faces = dict()
for ti, tri in enumerate(tris):
if visited[ti]:
continue
face = [ti]
visited[ti] = True
myvisit = set()
myvisit.add(ti)
q = deque()
q.append(ti)
while q:
tvisit = q.popleft()
for tn in tri_neighbors[tvisit]:
if tn >= 0 and tn not in myvisit:
if math.acos(trinormals[ti].dot(
trinormals[tn])) <= merge_tol:
face.append(tn)
myvisit.add(tn)
q.append(tn)
for t in myvisit:
visited[t] = True
cohesive_faces[ti] = face
output = []
for t, face in cohesive_faces.items():
n = trinormals[t]
R = so3.canonical(n)
if len(face) > 1:
#project face onto the canonical basis
faceverts = set()
for t in face:
faceverts.add(tris[t][0])
faceverts.add(tris[t][1])
faceverts.add(tris[t][2])
faceverts = list(faceverts)
xypts = [so3.apply(so3.inv(R), verts[v])[1:3] for v in faceverts]
try:
ch = ConvexHull(xypts)
face = [faceverts[v] for v in ch.vertices]
except Exception:
print("Error computing convex hull of", xypts)
print("Vertex indices", faceverts)
print("Vertices", [verts[v] for v in faceverts])
else:
face = tris[face[0]]
comproj = np.array(so3.apply(so3.inv(R), com)[1:3])
stable = True
for vi in range(len(face)):
vn = (vi + 1) % len(face)
a, b = face[vi], face[vn]
pa = np.array(so3.apply(so3.inv(R), verts[a])[1:3])
pb = np.array(so3.apply(so3.inv(R), verts[b])[1:3])
#check distance from com
elen = np.linalg.norm(pb - pa)
if elen == 0:
continue
sign = np.cross(pb - pa, comproj - pa) / elen
if sign < stability_tol:
stable = False
break
if stable:
output.append([verts[i] for i in face])
return output