def easy_obstacles(): start = np.array([-pi / 4, pi / 4]) goal = np.array([pi / 4, -pi / 4]) dim_box = np.array([[0.3, 0.6, 0.3], [0.3, 0.3, 0.3]]) obstacles = [box([0.3, 0.6, 0.3]), box([0.3, 0.3, 0.3])] obstacles[0].apply_translation([1.4, 0, 0]) obstacles[1].apply_translation([-0.5, 0, 0]) return start, goal, dim_box, obstacles
def hard_obstacles(): sign_matrix = [[1, 1], [1, -1], [-1, 1], [-1, -1]] start = np.array([-pi / 2, 0]) goal = np.array([pi / 2, 0]) d = 0.3 dim_box = np.array([[d, d, d], [d, d, d], [d, d, d], [d, d, d]]) obstacles = [box(dim_box[i]) for i in range(0, 4)] theta = [] c = 1.0 for i in range(0, 4): random.seed() theta.append(random.randint(0, 360)) r = 0.2 for i in range(0, 4): s1 = sign_matrix[i][0] s2 = sign_matrix[i][1] obstacles[i].apply_translation([ c * s1 + r * math.cos(math.radians(theta[i])), c * s2 + r * math.sin(math.radians(theta[i])), 0 ]) return start, goal, dim_box, obstacles, theta
def __init__( self, batch_size=2, radius=0.2, camextr=None, hand_links=None, debug=True, random_rot=True, z_off=0.5, y_off=0, x_off=0, mesh_type="box", ): super().__init__() self.batch_size = batch_size if mesh_type == "box": box = tricreation.box([1, 1, 1]) faces = torch.Tensor(np.array(box.faces)) verts_loc = torch.Tensor(np.array(box.vertices)) elif mesh_type == "sphere": icomesh = py3dutils.ico_sphere(2) verts_loc = icomesh.verts_padded()[0] faces = icomesh.faces_padded()[0] else: raise ValueError(f"{mesh_type} not in [sphere|box]") # Normalize and save verts norm_verts = normalize.normalize_verts(verts_loc, 1) self.register_buffer( "verts", norm_verts.unsqueeze(0).repeat(batch_size, 1, 1) ) self.register_buffer( "faces", faces.unsqueeze(0).repeat(batch_size, 1, 1) ) # Initialize translation and rotation rot_vecs = torch.stack( [rotutils.get_rotvec6d(random_rot) for _ in range(batch_size)] ) # Initialize rotation parameters self.rot_vecs = torch.nn.Parameter(rot_vecs, requires_grad=True) # Initialize scale and translation self.trans = torch.nn.Parameter( norm_verts.new_zeros(batch_size, 3) + norm_verts.new([x_off, y_off, z_off]).view(1, 3), requires_grad=True, ) # Scale is shared for all object views self.scale = torch.nn.Parameter( torch.Tensor([radius]), requires_grad=True )
def get_link_mesh(self, tm): init_pose = tm.visuals[0].origin if tm.visuals[0].geometry.cylinder is not None: length = tm.visuals[0].geometry.cylinder.length radius = tm.visuals[0].geometry.cylinder.radius mesh = cylinder(radius, length) mesh.visual.face_color = tm.visuals[0].material.color return init_pose, mesh else: ext = tm.visuals[0].geometry.box.size mesh = box(ext) mesh.visual.face_colors = tm.visuals[0].material.color return init_pose, mesh
def box( radius=0.0625, irregularity=0., extents=None, subdivisions=3, seed=None, ): """Generate an irregular mesh from a box. Args: radius: maximum radius of the object's bounding sphere. irregularity: irregularity measure, between 0 and 1. extents: ratio between extents. (Adimensional, scaled by radius.) subdivisions: number of subdivisions to perform to the box mesh. (A subdivision replaces each face with four smaller faces.) """ random = np.random.default_rng(seed) extents = extents or (1, 1 / 2, 1 / 3) extents = np.array(extents) * 2 * radius / np.linalg.norm(extents) mesh = creation.box(extents=extents) if irregularity > 0: # Add noise mesh.vertices += stats.truncnorm.rvs( -1 / irregularity, 1 / irregularity, loc=0, scale=irregularity * radius, size=mesh.vertices.shape, random_state=random, ) for i in range(subdivisions): nv = mesh.vertices.shape[0] mesh = mesh.subdivide() if irregularity > 0: # Add noise to the new vertices mesh.vertices[nv:] += stats.truncnorm.rvs( -1 / irregularity, 1 / irregularity, loc=0, scale=irregularity * radius * 2**(-(i + 1)), size=mesh.vertices[nv:].shape, random_state=random, ) mesh = mesh.convex_hull mesh.apply_translation(-mesh.center_mass) # pylint: disable=no-member factor = 2 * radius / max(mesh.bounding_box_oriented.extents) # pylint: disable=no-member if factor < 1: mesh.apply_transform(transformations.scale_matrix(factor)) # pylint: disable=no-member return mesh
def check_rooted(verts, faces): # Load up the mesh mesh = tm.Trimesh(vertices=verts, faces=faces) # Switch from y-up to z-up mesh.vertices = mesh.vertices[:, [0, 2, 1]] mesh.fix_normals() # Find the height of the ground plane z_ground = mesh.bounds[0][2] # Extract the individual cuboid parts comps = mesh.split().tolist() # Also create a thin box for the ground plane ground = box( extents = [10, 10, 0.01], transform = [ [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, z_ground - 0.01/2], [0, 0, 0, 1] ] ) comps.insert(0, ground) # Detect (approximate) intersections between parts # collision_dist = 0.005 * mesh.scale # collision_dist = 0.01 * mesh.scale collision_dist = 0.02 * mesh.scale adjacencies = {comp_index : [] for comp_index in range(len(comps))} manager = CollisionManager() for i in range(len(comps)-1): manager.add_object(str(i), comps[i]) for j in range(i+1, len(comps)): dist = manager.min_distance_single(comps[j]) if (dist < collision_dist): adjacencies[i].append(j) adjacencies[j].append(i) manager.remove_object(str(i)) # Run a DFS starting from the ground, check if everything is reachable visited = [False for comp in comps] stack = [0] # Index of 'ground' while len(stack) > 0: nindex = stack.pop() visited[nindex] = True for cindex in adjacencies[nindex]: if not visited[cindex]: stack.append(cindex) return all(visited)
def chunk_box(xyz, chunk_size=[1, 1, 1]): """Create a trimesh box of a specified size Parameters ---------- xyz : array-like 3-element array for the lower corner of the box chunk_size : array-like, optional 3-element array giving box dimensions, by default [1, 1, 1] Returns ------- trimesh.mesh """ xyz_offset = xyz + np.array(chunk_size) / 2 return creation.box(chunk_size, _tmat(xyz_offset))
def xarm_easy_obstacles(): random.seed() x = random.randrange(300, 500) x /= 1000 start = np.array([-pi / 4, pi / 2.5, -pi / 1.5, pi / 2, 0, 0]) goal = np.array([pi / 4, pi / 2.5, -pi / 1.5, pi / 2, 0, 0]) dim_box = np.array([[0.1, 0.1, 0.5]]) obstacles = [box(dim_box[0])] obstacles[0].apply_translation([x, 0, 0.25]) table = cylinder(radius=0.7, height=0.02) table.apply_translation([0, 0, -0.015]) obstacles.append(table) return start, goal, dim_box, obstacles, x
def xarm_hard_obstacles(): x, z = 0.6, 1. start = np.array([-pi / 4, pi / 4, -pi / 2, 0, 0, 0]) goal = np.array([pi / 4, pi / 4, -pi / 2, 0, 0, 0]) dim_box = [[0.1, 0.1, 0.5], [0.1, 0.1, 0.1], [0.1, 0.6, 0.6], [0.1, 0.3, 0.1]] obstacles = [box(dim_box[i]) for i in range(0, 4)] obstacles[0].apply_translation([x, 0, 0.25]) obstacles[1].apply_translation([0.3, 0, z]) obstacles[2].apply_translation([-0.2, 0, 0.3]) obstacles[3].apply_translation([x, 0, 0.5]) table = cylinder(radius=0.7, height=0.02) table.apply_translation([0, 0, -0.015]) obstacles.append(table) return start, goal, dim_box, obstacles, x, z
def _trimesh_from_basic_shape(self, shape: Shape, vertex_colors=None): """ Get trimesh from shape for visual meshes, spheres, or boxes. Return None if it is not a basic shape. """ if shape.get_user_data() is not None: visual_mesh = shape.get_user_data().get('visual_mesh', None) if visual_mesh is not None: if not isinstance(visual_mesh, Trimesh): raise NotImplementedError( 'Only trimesh scene or trimesh instances are supported.' ) if hasattr(visual_mesh.visual, 'material'): visual_mesh.visual.material.kwargs['Ns'] = np.abs( visual_mesh.visual.material.kwargs['Ns']) return visual_mesh gtype = shape.get_geometry_type() if not gtype in [GeometryType.SPHERE, GeometryType.BOX]: return None geom = uv_sphere( shape.get_sphere_radius(), self.spheres_count) if gtype == GeometryType.SPHERE else box( 2 * shape.get_box_half_extents()) if vertex_colors is not None: geom.visual.vertex_colors = vertex_colors return geom
def chunk_box(xyz, chunk_size=[1, 1, 1]): """Create a trimesh box for a single chunk""" xyz_offset = xyz + np.array(chunk_size) / 2 return creation.box(chunk_size, _tmat(xyz_offset))
from bfieldtools.mesh_calculus import mass_matrix # domain = 'sphere' # domain = 'cube' domain = "combined" if domain == "sphere": from trimesh.creation import icosphere mesh1 = icosphere(3, 0.65) mesh2 = icosphere(3, 0.8) elif domain == "cube": from trimesh.creation import box from trimesh.smoothing import filter_laplacian mesh1 = box((1.0, 1.0, 1.0)) mesh2 = box((1.5, 1.5, 1.5)) for i in range(4): mesh1 = mesh1.subdivide() mesh2 = mesh2.subdivide() mesh1 = filter_laplacian(mesh1) mesh2 = filter_laplacian(mesh2, 0.9) elif domain == "combined": from trimesh.creation import icosphere mesh1 = icosphere(4, 0.65) mesh2 = load_example_mesh("cube_fillet") mesh2.vertices -= mesh2.vertices.mean(axis=0) mesh2.vertices *= 0.15