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)