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