def create_mesh_box(env, center, half_extents, name="box"):
box = rave.RaveCreateKinBody(env, '')
rx, ry, rz = half_extents
verts = np.array([
[-rx, -ry, -rz],
[-rx, -ry, rz],
[-rx, ry, -rz],
[-rx, ry, rz],
[rx, -ry, -rz],
[rx, -ry, rz],
[rx, ry, -rz],
[rx, ry, rz]])
faces= [
[0,1,2],
[3,1,2],
[0,1,4],
[5,1,4],
[0,2,4],
[6,2,4],
[7,6,5],
[4,6,5],
[7,6,3],
[2,6,3],
[7,5,3],
[1,5,3]]
box.SetName(name)
box.InitFromTrimesh(rave.TriMesh(verts, faces), True)
trans = np.eye(4)
trans[:3,3] = center
box.SetTransform(trans)
env.Add(box)
return box
def create_convex_soup(cloud, env, name="convexsoup"):
xyz = cloud.to2dArray()
indss = cloudprocpy.convexDecomp(cloud, .03)
geom_infos = []
for (i, inds) in enumerate(indss):
if len(inds) < 100:
continue # openrave is slow with a lot of geometries
origpts = xyz[inds]
hullpoints, hullinds = calc_hull(origpts)
if hullpoints is None:
print "failed to get convex hull!"
continue
gi = rave.KinBody.GeometryInfo()
gi._meshcollision = rave.TriMesh(hullpoints, hullinds)
gi._type = rave.GeometryType.Trimesh
gi._vAmbientColor = np.random.rand(3) / 2
geom_infos.append(gi)
body = rave.RaveCreateKinBody(env, '')
body.SetName(name)
body.InitFromGeometries(geom_infos)
env.Add(body)
def add_kinbody(self, vertices, triangles, name=None, check_collision=False):
"""
:param vertices: list of 3d np.ndarray corresponding to points in the mesh
:param triangles: list of 3d indices corresponding to vertices
:param name: name of the kinbody to be added
:param check_collision: if True, will not add kinbody if it collides with the robot
:return False if check_collision=True and there is a collision
"""
name = name if name is not None else 'kinbody'+str(time.time())
self.added_kinbody_names.append(name)
body = rave.RaveCreateKinBody(self.env, "")
body.InitFromTrimesh(trimesh=rave.TriMesh(vertices, triangles), draw=True)
body.SetName(name)
self.env.Add(body)
randcolor = np.random.rand(3)
body.GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
body.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
if check_collision:
if self.env.CheckCollision(self.robot, body):
self.env.Remove(body)
self.added_kinbody_names = self.added_kinbody_names[:-1]
return False
return True
def setup_obj_rave(obj_cloud_xyz, obj_name):
rave_env = Globals.pr2.env
pc_down = voxel_downsample(obj_cloud_xyz, .02)
body = rave.RaveCreateKinBody(rave_env, '')
body.SetName(obj_name) #might want to change this name later
delaunay = scipy.spatial.Delaunay(pc_down)
body.InitFromTrimesh(rave.TriMesh(delaunay.points, delaunay.convex_hull),
True)
rave_env.Add(body)
return body
def trimesh_cylinder(env, name, z_range, r):
trimesh_cylinder = rave.RaveCreateKinBody(env, '')
trimesh_cylinder.SetName(name)
check_range_z = z_range
check_range_r = r
circle_divide = 36
cylinder_trimesh_vertices = np.zeros((circle_divide * 2 + 2, 3))
cylinder_trimesh_indices = np.zeros((circle_divide * 4, 3))
for t in range(circle_divide * 2):
if t < circle_divide:
cz = check_range_z[0]
else:
cz = check_range_z[1]
cos_theta = math.cos(
(t % circle_divide) * (360.0 / circle_divide) * deg_to_rad)
sin_theta = math.sin(
(t % circle_divide) * (360.0 / circle_divide) * deg_to_rad)
cx = check_range_r * cos_theta
cy = check_range_r * sin_theta
cylinder_trimesh_vertices[t:t + 1, 0:3] = np.array((cx, cy, cz))
cylinder_trimesh_vertices[circle_divide * 2, 0:3] = np.array(
(0, 0, check_range_z[0]))
cylinder_trimesh_vertices[circle_divide * 2 + 1, 0:3] = np.array(
(0, 0, check_range_z[1]))
for t in range(circle_divide * 2):
if t < circle_divide:
center_index = circle_divide * 2
vertex1 = t % circle_divide
vertex2 = (t + 1) % circle_divide
vertex3 = t % circle_divide + circle_divide
cylinder_trimesh_indices[2 * t:2 * t + 1, 0:3] = np.array(
[center_index, vertex2, vertex1])
cylinder_trimesh_indices[2 * t + 1:2 * (t + 1), 0:3] = np.array(
[vertex1, vertex2, vertex3])
else:
center_index = circle_divide * 2 + 1
vertex1 = t % circle_divide + circle_divide
vertex2 = (t + 1) % circle_divide + circle_divide
vertex3 = (t + 1) % circle_divide
cylinder_trimesh_indices[2 * t:2 * t + 1, 0:3] = np.array(
[center_index, vertex1, vertex2])
cylinder_trimesh_indices[2 * t + 1:2 * (t + 1), 0:3] = np.array(
[vertex1, vertex3, vertex2])
trimesh_cylinder.InitFromTrimesh(
rave.TriMesh(cylinder_trimesh_vertices, cylinder_trimesh_indices),
False)
return trimesh_cylinder
def trimesh_from_point_cloud(cloud):
"""
Converts a PCL point cloud into a OpenRAVE trimesh
@type cloud: pcl.Cloud
@param cloud: The PCL cloud
@rtype: orpy.Trimesh
@return: The OpenRAVE trimesh
"""
points = np.asarray(cloud)
hull = scipy.spatial.ConvexHull(points)
hull = scipy.spatial.ConvexHull(points[hull.vertices])
criros.spalg.counterclockwise_hull(hull)
return orpy.TriMesh(hull.points, hull.simplices)
def load_object(self, obj_path):
"""
Load object into the env.
@type obj_path: string
@param obj_path: path to the object (.ply .obj)
@rtype: openravepy.KinBody
@return: object
"""
mesh = trimesh.load(obj_path)
with self.env:
body = orpy.RaveCreateKinBody(self.env, '')
body.InitFromTrimesh(trimesh=orpy.TriMesh(mesh.vertices, mesh.faces), draw=True)
body.SetName("chair")
self.env.AddKinBody(body)
body.Enable(True)
return body
import numpy as np
import scipy.spatial
env = openravepy.Environment()
point_cloud = np.random.randn(12,3)
gi = openravepy.KinBody.GeometryInfo()
gi._meshcollision = openravepy.Trimesh
body = openravepy.RaveCreateKinBody(env,'')
body.SetName("cup")
def calc_hull(points):
if len(points) == 1:
return points, []
else:
delaunay = scipy.spatial.Delaunay(point_cloud)
return delaunay.points, delaunay.convex_hull
delaunay = scipy.spatial.Delaunay(point_cloud)
body.InitFromTrimesh(openravepy.TriMesh(delaunay.points, delaunay.convex_hull), True)
env.Add(body)
print delaunay.points
print delaunay.convex_hull
print len(delaunay.points)
#import trajoptpy
#cc = trajoptpy.GetCollisionChecker(env)
#cc.AllVsAll()
import openravepy
import numpy as np
env = openravepy.Environment()
##### Spheres ####
if False:
body = openravepy.RaveCreateKinBody(env, '')
body.SetName("cup")
point_cloud = np.random.randn(30, 3)
radii = np.ones(len(point_cloud)) * .003
body.InitFromSpheres(np.c_[point_cloud, radii], True)
env.Add(body)
#### Convex hull mesh #####
if True:
delaunay = scipy.spatial.Delaunay(point_cloud)
body2 = openravepy.RaveCreateKinBody(env, '')
body2.SetName("cup2")
body2.InitFromTrimesh(
openravepy.TriMesh(delaunay.vertices, delaunay.convex_hull.flatten()))
env.Add(body2)
def create_trimesh(env, verts, faces, name="mesh"):
body = rave.RaveCreateKinBody(env, "")
body.InitFromTrimesh(trimesh=rave.TriMesh(verts, faces),draw=True)
body.SetName(name)
env.Add(body)
return body
def trimesh_fan(env, name, z_range, angle_range, r, origin=(0, 0)):
min_angle = angle_range[0]
max_angle = angle_range[1]
trimesh_fan = rave.RaveCreateKinBody(env, '')
trimesh_fan.SetName(name)
check_range_z = z_range
check_range_r = r
arc_vertices_num = (max_angle - min_angle) / 10 + 1
fan_trimesh_vertices = np.zeros((arc_vertices_num * 2 + 2, 3))
fan_trimesh_indices = np.zeros(((arc_vertices_num - 1) * 4 + 4, 3))
for t in range(arc_vertices_num * 2):
if t < arc_vertices_num:
cz = check_range_z[0]
else:
cz = check_range_z[1]
cos_theta = math.cos(
((t % arc_vertices_num) * 10 + min_angle) * deg_to_rad)
sin_theta = math.sin(
((t % arc_vertices_num) * 10 + min_angle) * deg_to_rad)
cx = check_range_r * cos_theta + origin[0]
cy = check_range_r * sin_theta + origin[1]
fan_trimesh_vertices[t:t + 1, 0:3] = np.array((cx, cy, cz))
fan_trimesh_vertices[arc_vertices_num * 2, 0:3] = np.array(
(origin[0], origin[1], check_range_z[0]))
fan_trimesh_vertices[arc_vertices_num * 2 + 1, 0:3] = np.array(
(origin[0], origin[1], check_range_z[1]))
lower_center_index = arc_vertices_num * 2
upper_center_index = arc_vertices_num * 2 + 1
for t in range(arc_vertices_num * 2):
if t < arc_vertices_num - 1:
vertex1 = t % arc_vertices_num
vertex2 = (t + 1) % arc_vertices_num
vertex3 = t % arc_vertices_num + arc_vertices_num
fan_trimesh_indices[2 * t:2 * t + 1, 0:3] = np.array(
[lower_center_index, vertex2, vertex1])
fan_trimesh_indices[2 * t + 1:2 * t + 2,
0:3] = np.array([vertex1, vertex2, vertex3])
elif t > arc_vertices_num - 1 and t < 2 * arc_vertices_num - 1:
vertex1 = t % arc_vertices_num + arc_vertices_num
vertex2 = (t + 1) % arc_vertices_num + arc_vertices_num
vertex3 = (t + 1) % arc_vertices_num
fan_trimesh_indices[2 * t - 2:2 * t - 1, 0:3] = np.array(
[upper_center_index, vertex1, vertex2])
fan_trimesh_indices[2 * t - 1:2 * t,
0:3] = np.array([vertex1, vertex3, vertex2])
fan_trimesh_indices[(arc_vertices_num - 1) * 4:(arc_vertices_num - 1) * 4 +
1, 0:3] = np.array(
[lower_center_index, 0, arc_vertices_num])
fan_trimesh_indices[(arc_vertices_num - 1) * 4 +
1:(arc_vertices_num - 1) * 4 + 2, 0:3] = np.array([
lower_center_index, arc_vertices_num,
upper_center_index
])
fan_trimesh_indices[(arc_vertices_num - 1) * 4 +
2:(arc_vertices_num - 1) * 4 + 3, 0:3] = np.array([
upper_center_index, 2 * arc_vertices_num - 1,
arc_vertices_num - 1
])
fan_trimesh_indices[(arc_vertices_num - 1) * 4 +
3:(arc_vertices_num - 1) * 4 + 4, 0:3] = np.array([
upper_center_index, arc_vertices_num - 1,
lower_center_index
])
trimesh_fan.InitFromTrimesh(
rave.TriMesh(fan_trimesh_vertices, fan_trimesh_indices), False)
trimesh_fan.GetLinks()[0].GetGeometries()[0].SetTransparency(0.2)
return trimesh_fan
def simplify_file(filepath):
from sklearn import cluster
env = rave.Environment()
env.Load(filepath)
env.SetViewer('qtcoin')
### get bodies, then remove from env
print('Getting bodies')
bodies = env.GetBodies()
for body in bodies:
env.Remove(body)
### get all vertices and indices
print('Getting all vertices and indices')
all_vertices, all_indices = [], []
for body in bodies:
if body.IsRobot(): continue
for link in body.GetLinks():
T_link = link.GetTransform()
for geom in [g for g in link.GetGeometries() if g.IsModifiable()]:
orig_tri_mesh = geom.GetCollisionMesh()
all_indices += (orig_tri_mesh.indices + len(all_vertices)).tolist()
v = orig_tri_mesh.vertices
v = T_link[:3,:3].dot(v.T).T + T_link[:3,3]
all_vertices += v.tolist()
all_indices_array = np.asarray(all_indices)
### kmeans on vertices
print('KMeans')
kmeans = cluster.MiniBatchKMeans(n_clusters=int(len(all_vertices) / 1e4),
verbose=0)
kmeans.fit(all_vertices)
all_vertices_labels = np.copy(kmeans.labels_)
print('Creating clusters')
num_clusters = np.max(all_vertices_labels) + 1
indices_clusters = [[] for _ in xrange(num_clusters)]
vertices_clusters = [[] for _ in xrange(num_clusters)]
added_indices = set() # keep track of triangles that have already been added to a cluster
for c in xrange(num_clusters): # for each cluster
print('Cluster {0} / {1}'.format(c+1, num_clusters))
c_vertices_indices = (all_vertices_labels == c).nonzero()[0] # all vertices indices in cluster
total_nonzero, total_indices = 0, 0
c_indices = [] # triangles involved in this cluster
for index in c_vertices_indices.flatten():
start_nonzero = time.time()
# indices_with_index = (all_indices_array[:,0] == index).nonzero()[0]
# indices_with_index = (all_indices_array == index).max(axis=1).nonzero()[0] # which triangle contain index
indices_with_index = set()
for i in xrange(3):
indices_with_index_i = (all_indices_array[:,i] == index).nonzero()[0]
indices_with_index.update(indices_with_index_i.tolist())
indices_with_index = list(indices_with_index)
total_nonzero += time.time() - start_nonzero
start_indices = time.time()
# only add triangle to one cluster
for indice in all_indices_array[indices_with_index].tolist():
indice_sorted = sorted(indice)
if tuple(indice_sorted) not in added_indices:
c_indices.append(indice)
added_indices.add(tuple(indice_sorted))
total_indices += time.time() - start_indices
### the indices need to be made starting from 0 and sequential
c_num_vertices = len(set(np.asarray(c_indices).flatten()))
c_remapping = dict([(old, new) for new, old in enumerate(set(np.asarray(c_indices).flatten()))])
c_indices_remapped = [] # new triangle indexing
for indice in c_indices:
c_indices_remapped.append([c_remapping[old_index] for old_index in indice])
c_vertices_remapped = [None for _ in xrange(c_num_vertices)] # new vertex ordering
for index in set(np.asarray(c_indices).flatten()):
assert(c_vertices_remapped[c_remapping[index]] is None)
c_vertices_remapped[c_remapping[index]] = all_vertices[index]
assert(None not in c_vertices_remapped)
assert(np.max(c_indices_remapped) < len(c_vertices_remapped))
assert(len(set(np.asarray(c_indices_remapped).flatten().tolist())) == len(c_vertices_remapped))
indices_clusters[c] = c_indices_remapped
vertices_clusters[c] = c_vertices_remapped
### create new rave object
print('Creating openrave objects')
for c, (indices_c, vertices_c) in enumerate(zip(indices_clusters, vertices_clusters)):
name = 'cluster_{0}'.format(c)
body = rave.RaveCreateKinBody(env, "")
body.InitFromTrimesh(trimesh=rave.TriMesh(vertices_c, indices_c), draw=True)
body.SetName(name)
env.Add(body)
randcolor = np.random.rand(3)
# randcolor = [ 0.6, 0.3, 0.] # [0., 1., 1.] # [1, 0, 0]
body.GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
body.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
dirname = os.path.dirname(filepath)
basename = os.path.basename(filepath)
basename_simplified = basename.split('.')[0] + '_simplified.zae'
filename_simplified = os.path.join(dirname, basename_simplified)
env.Save(filename_simplified, rave.Environment.SelectionOptions.Everything)
