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 determine_grasps(obj_grasp_configs,
kit_t_objects: Dict[str, Transform],
min_clearance,
debug=False,
debug_view_res=(800, 500)):
if debug:
scene = trimesh.Scene()
for name, mesh, *_ in obj_grasp_configs:
scene.add_geometry(mesh,
transform=kit_t_objects[name].matrix,
geom_name=name)
scene.show(resolution=debug_view_res)
lense = trimesh.load_mesh(
Path(__file__).parent.parent / 'stl' / 'lense.stl')
lense.apply_scale(1e-3)
col_tool = CollisionManager()
col_tool.add_object('lense', lense)
col_objects = CollisionManager()
for name, mesh, *_ in obj_grasp_configs:
col_objects.add_object(name,
mesh,
transform=kit_t_objects[name].matrix)
worst_possible_clearance = np.inf
grasps = []
candidates = [*obj_grasp_configs]
clearances = []
i = 0
while candidates:
candidate = candidates[i]
name, mesh, tcp_t_obj, grasp_start_width, grasp_end_width, sym_deg, sym_offset = candidate
kit_t_obj = kit_t_objects[name]
# move away the grasp obj to ignore obj-tcp collision
col_objects.set_transform(name, Transform(p=(1000, 0, 0)).matrix)
finger_grasp_volume = build_finger_grasp_volume(grasp_start_width)
col_tool.add_object('finger_grasp_volume', finger_grasp_volume)
kit_t_tcp = kit_t_obj @ tcp_t_obj.inv
ts = rotational_grasp_symmetry(kit_t_tcp, sym_deg, sym_offset)
dists, dists_name = [], []
for t in ts:
col_tool.set_transform('lense', t.matrix)
col_tool.set_transform('finger_grasp_volume', t.matrix)
dist, dist_names = col_objects.min_distance_other(
col_tool, return_names=True)
if dist < worst_possible_clearance:
worst_possible_clearance = dist
dists.append(dist)
dists_name.append(dist_names[0])
di = np.argmax(dists).item()
kit_t_tcp, dist, dist_name = ts[di], dists[di], dists_name[di]
tcp_t_obj = kit_t_tcp.inv @ kit_t_obj
col_tool.remove_object('finger_grasp_volume')
if dist > min_clearance:
grasps.append(
(name, tcp_t_obj, grasp_start_width, grasp_end_width))
candidates.remove(candidate)
clearances.append(dist)
i = 0
if debug:
print(f'{name}: {dist:.3f}')
scene.add_geometry(lense,
transform=kit_t_tcp.matrix,
geom_name='lense')
scene.add_geometry(finger_grasp_volume,
transform=kit_t_tcp.matrix,
geom_name='finger_grasp_volume')
scene.show(resolution=debug_view_res)
scene.delete_geometry([name, 'lense', 'finger_grasp_volume'])
else:
if debug:
print(f'!! {name}: {dist_name} {dist:.3f} - changing order')
# move the grasp object back in place
col_objects.set_transform(name, kit_t_obj.matrix)
i += 1
if i == len(candidates):
raise RuntimeError('cant find solution with this clearance')
return grasps
def obj2urdf(input_file, output_dir, density=1):
if not os.path.exists(output_dir):
os.makedirs(output_dir)
# Load up the mesh
mesh = tm.load(input_file)
# Switch from y-up to z-up
mesh.vertices = mesh.vertices[:, [0, 2, 1]]
mesh.fix_normals()
# Extract the individual cuboid parts
comps = mesh.split().tolist()
# Detect (approximate) intersections between parts, to use for building joints
# 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))
# Compute connected components
conn_comps = []
visited = [False for _ in comps]
while not all(visited):
conn_comp = set([])
start_idx = visited.index(False)
stack = [start_idx]
while len(stack) > 0:
idx = stack.pop()
visited[idx] = True
conn_comp.add(idx)
for nidx in adjacencies[idx]:
if not visited[nidx]:
stack.append(nidx)
conn_comps.append(list(conn_comp))
# We export one URDF object file per connected component
for i,conn_comp in enumerate(conn_comps):
# Re-center this connected component
ccmesh = meshconcat([comps[j] for j in conn_comp])
c = ccmesh.centroid
transmat = [
[1, 0, 0, -c[0]],
[0, 1, 0, -c[1]],
[0, 0, 1, -c[2]],
[0, 0, 0, 1]
]
for j in conn_comp:
comps[j].apply_transform(transmat)
ccmesh.apply_transform(transmat)
# Also, record where to start this mesh in the simulation
# That's the x,y coords of the centroid, and -bbox bottom for the z (so it sits on the ground)
# And the bbox diagonal (we use this for error thresholding)
metadata = {
'start_pos': [c[0], c[1], -ccmesh.bounds[0][2]],
'scale': ccmesh.volume
}
with open(f'{output_dir}/assembly_{i}.json', 'w') as f:
f.write(json.dumps(metadata))
# Build a directed tree by DFS
root_idx = conn_comp[0]
root = {'id': root_idx, 'children': []}
fringe = [root]
visited = set([root['id']])
while len(fringe) > 0:
node = fringe.pop()
for neighbor in adjacencies[node['id']]:
if not (neighbor in visited):
child_node = {'id': neighbor, 'children': []}
node['children'].append(child_node)
visited.add(child_node['id'])
fringe.append(child_node)
# Build up the URDF data structure
urdf_root = xml.Element('robot')
urdf_root.set('name', 'part_graph_shape')
# Links
for j in conn_comp:
comp = comps[j]
link = xml.SubElement(urdf_root, 'link')
link.set('name', f'part_{j}')
visual = xml.SubElement(link, 'visual')
geometry = xml.SubElement(visual, 'geometry')
mesh = xml.SubElement(geometry, 'mesh')
mesh.set('filename', f'{output_dir}/part_{j}.stl')
material = xml.SubElement(visual, 'material')
material.set('name', 'gray')
color = xml.SubElement(material, 'color')
color.set('rgba', '0.5 0.5 0.5 1')
collision = xml.SubElement(link, 'collision')
geometry = xml.SubElement(collision, 'geometry')
mesh = xml.SubElement(geometry, 'mesh')
mesh.set('filename', f'{output_dir}/part_{j}.stl')
inertial = xml.SubElement(link, 'inertial')
mass = xml.SubElement(inertial, 'mass')
mass.set('value', str(comp.volume * density))
inertia = xml.SubElement(inertial, 'inertia')
inertia.set('ixx', '1.0')
inertia.set('ixy', '0.0')
inertia.set('ixz', '0.0')
inertia.set('iyy', '1.0')
inertia.set('iyz', '0.0')
inertia.set('izz', '1.0')
# Joints
fringe = [root]
while len(fringe) > 0:
node = fringe.pop()
for child_node in node['children']:
joint = xml.SubElement(urdf_root, 'joint')
joint.set('name', f'{node["id"]}_to_{child_node["id"]}')
joint.set('type', 'fixed')
parent = xml.SubElement(joint, 'parent')
parent.set('link', f'part_{node["id"]}')
child = xml.SubElement(joint, 'child')
child.set('link', f'part_{child_node["id"]}')
origin = xml.SubElement(joint, 'origin')
origin.set('xyz', '0 0 0')
fringe.append(child_node)
# Save URDF file to disk
# Have to make sure to split it into multiple lines, otherwise Bullet's URDF parser will
# throw an error trying to load really large files as a single line...
xmlstring = xml.tostring(urdf_root, encoding='unicode')
xmlstring = '>\n'.join(xmlstring.split('>'))
with open(f'{output_dir}/assembly_{i}.urdf', 'w') as f:
f.write(xmlstring)
# Write the parts to disk as STL files for the URDF to refer to
for i,comp in enumerate(comps):
comp.export(f'{output_dir}/part_{i}.stl')
class UR5(RealVectorSpace):
def __init__(self, obstacles) -> None:
self.robot = URDF.load('data/ur5/ur5.urdf')
self.spaces = RealVectorSpace(6)
self.robot_cm = CollisionManager()
self.env_cm = CollisionManager()
cfg = self.get_config([0, 0, 0, 0, 0, 0])
self.init_poses = [0 for i in range(6)]
#print(cfg)
fk = self.robot.collision_trimesh_fk(cfg=cfg)
#fk = self.robot.visual_trimesh_fk(cfg=cfg)
# adding robot to the scene
for i, tm in enumerate(fk):
pose = fk[tm]
name = "link_" + str(i + 1)
self.init_poses.append(pose)
self.robot_cm.add_object(name, tm, pose)
table = cylinder(radius=0.7, height=0.02)
table.apply_translation([0, 0, -0.015])
obstacles.append(table)
for i, ob in enumerate(obstacles):
self.env_cm.add_object("obstacle_" + str(i), ob)
def spaces(self):
return self.spaces
def robot(self):
return self.robot
def is_in_collision(self, q):
cfg = self.get_config(q)
fk = self.robot.collision_trimesh_fk(cfg=cfg)
# adding robot to the scene
for i, tm in enumerate(fk):
pose = fk[tm]
self.robot_cm.set_transform("link_" + str(i + 1), pose)
# print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform())
return self.robot_cm.in_collision_other(self.env_cm)
def is_valid(self, q, qe=None, num_checks=None):
if qe is None or num_checks is None:
res = self.is_in_collision(q)
return not (res)
else:
for i in range(num_checks):
q_i = self.get_qnew(q, qe, i / num_checks)
res = self.is_in_collision(q_i)
if res:
return False
return True
def get_config(self, q):
if len(self.robot.actuated_joints) != len(q):
raise Exception("Wrong dimensions of q")
cfg = {}
for i in range(len(q)):
cfg[self.robot.actuated_joints[i].name] = q[i]
return cfg
def get_link_mesh(self, tm):
print(tm.visuals)
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 show(self, q=None, obstacles=None, use_collision=False):
cfg = self.get_config(q)
#print(cfg)
if use_collision:
fk = self.robot.collision_trimesh_fk(cfg=cfg)
else:
fk = self.robot.visual_trimesh_fk(cfg=cfg)
scene = pyrender.Scene()
# adding robot to the scene
for tm in fk:
pose = fk[tm]
mesh = pyrender.Mesh.from_trimesh(tm, smooth=False)
scene.add(mesh, pose=pose)
# adding base box to the scene
table = cylinder(radius=0.7, height=0.02) #([0.5, 0.5, 0.02])
table.apply_translation([0, 0, -0.015])
table.visual.vertex_colors = [205, 243, 8, 255]
scene.add(pyrender.Mesh.from_trimesh(table))
# adding obstacles to the scene
for i, ob in enumerate(obstacles):
if i < len(obstacles) - 1:
ob.visual.vertex_colors = [255, 0, 0, 255]
else:
ob.visual.vertex_colors = [205, 243, 8, 255]
scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False))
pyrender.Viewer(scene, use_raymond_lighting=True)
def animate(self, q_traj=None, obstacles=None):
import time
fps = 10.0
cfgs = [self.get_config(q) for q in q_traj]
# Create the scene
fk = self.robot.visual_trimesh_fk(cfg=cfgs[0])
node_map = {}
init_pose_map = {}
scene = pyrender.Scene()
for tm in fk:
pose = fk[tm]
mesh = pyrender.Mesh.from_trimesh(tm, smooth=False)
node = scene.add(mesh, pose=pose)
node_map[tm] = node
# adding base box to the scene
#table = cylinder(radius=0.7, height=0.02) #([0.5, 0.5, 0.02])
#table.apply_translation([0, 0, -0.015])
#table.visual.vertex_colors = [205, 243, 8, 255]
#scene.add(pyrender.Mesh.from_trimesh(table))
cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414)
init_cam_pose = np.eye(4)
init_cam_pose[2, 3] = 2.5
scene.add(cam, pose=init_cam_pose)
# adding obstacles to the scene
for i, ob in enumerate(obstacles):
if i < len(obstacles) - 1:
ob.visual.vertex_colors = [255, 0, 0, 255]
else:
ob.visual.vertex_colors = [205, 243, 8, 255]
scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False))
# Pop the visualizer asynchronously
v = pyrender.Viewer(scene,
run_in_thread=True,
use_raymond_lighting=True)
time.sleep(1.0)
# Now, run our loop
i = 0
while v.is_active:
cfg = cfgs[i]
fk = self.robot.visual_trimesh_fk(cfg=cfg)
if i < len(cfgs) - 1:
i += 1
else:
i = 0
time.sleep(1.0)
v.render_lock.acquire()
for mesh in fk:
pose = fk[mesh]
node_map[mesh].matrix = pose
v.render_lock.release()
time.sleep(1.0 / fps)
d_cellname_poly = {}
for ix, vtp_path_i in enumerate(sorted(input_folder.glob('*.vtp'))):
poly = read_vtp_file(vtp_path_i)
d_cellname_poly[vtp_path_i.stem] = poly
write_stl_file(poly, (output_folder / "STL_converted" /
(vtp_path_i.stem + ".stl")))
cm = CollisionManager()
d_cellname_trimeshStl = {}
for ix, stl_i in enumerate(
sorted((output_folder / "STL_converted").glob('cell*.stl'))):
print('Adding stl -> {0}'.format(stl_i.name))
f = open(str(stl_i), 'r')
trimesh_stl = trimesh.load(f, file_type="stl")
d_cellname_trimeshStl[stl_i.stem] = trimesh_stl
cm.add_object(stl_i.stem, trimesh_stl)
is_collision, s_t_colliding_pairs = cm.in_collision_internal(return_names=True,
return_data=False)
print("cm.in_collision_internal = {0} \nwith colliding pairs {1} ".format(
is_collision, s_t_colliding_pairs))
if not is_collision:
print("No collision between the cells, exitting")
exit()
dd_cell1_cell2_contactinfo = DictOfDicts(
) #dict of dicts : key 1= cell1, key2 = cell2 , value = contactinfo object
l_vtp_columns = ['area', 'area_flat', 'area_eq', 'contact_area']
class TwoDOF(RealVectorSpace):
def __init__(self, obstacles) -> None:
self.robot = URDF.load('data/two_planar/2dof_planar.urdf')
self.spaces = RealVectorSpace(2)
self.robot_cm = CollisionManager()
self.env_cm = CollisionManager()
self.start_config = [0, 0]
self.traj = []
self.count_i = 0
self.curr_q = [0, 0]
cfg = self.get_config([0, 0])
fk = self.robot.link_fk(cfg=cfg)
self.init_poses = []
for i, tm in enumerate(fk):
pose = fk[tm]
init_pose, link_mesh = self.get_link_mesh(tm)
self.init_poses.append(init_pose)
self.robot_cm.add_object(
tm.name, link_mesh, np.matmul(pose, init_pose))
for i, ob in enumerate(obstacles):
self.env_cm.add_object("obstacle_" + str(i), ob)
self.env_cm.add_object("prediction_" + str(i), ob)
def set_trajectory(self, path):
self.traj = path
self.curr_q = path[0]
self.count_i = 0
# def get_next_q(self):
# if len(self.traj) == 0:
# return None
# self.traj.pop()
# self.curr_q = self.traj[-1]
# # print(self.traj)
# return self.curr_q
def get_next_q(self):
self.count_i += 1
if self.count_i >= len(self.traj):
return None
self.curr_q = self.traj[self.count_i]
return self.curr_q
def update_env(self, obstacles):
with threading.Lock():
for i, ob in enumerate(obstacles):
self.env_cm.remove_object("obstacle_" + str(i))
self.env_cm.add_object("obstacle_" + str(i), ob)
def update_predictions(self, predictions):
with threading.Lock():
for i, ob in enumerate(predictions):
self.env_cm.remove_object("prediction_" + str(i))
self.env_cm.add_object("prediction_" + str(i), ob)
def spaces(self):
return self.spaces
def robot(self):
return self.robot
def is_in_collision(self, q):
cfg = self.get_config(q)
fk = self.robot.link_fk(cfg=cfg)
# adding robot to the scene
for i, tm in enumerate(fk):
pose = fk[tm]
init_pose = self.init_poses[i]
self.robot_cm.set_transform(tm.name, np.matmul(pose, init_pose))
# print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform())
return self.robot_cm.in_collision_other(self.env_cm)
def distance(self, q):
cfg = self.get_config(q)
fk = self.robot.link_fk(cfg=cfg)
# adding robot to the scene
for i, tm in enumerate(fk):
pose = fk[tm]
init_pose = self.init_poses[i]
self.robot_cm.set_transform(tm.name, np.matmul(pose, init_pose))
# print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform())
return self.robot_cm.min_distance_other(self.env_cm)
def is_valid(self, q, qe=None, num_checks=None):
if qe is None or num_checks is None:
res = self.is_in_collision(q)
return not(res)
else:
for i in range(num_checks):
q_i = self.get_qnew(q, qe, i/num_checks)
res = self.is_in_collision(q_i)
if res:
return False
return True
def get_config(self, q):
if len(self.robot.actuated_joints) != len(q):
raise Exception("Wrong dimensions of q")
cfg = {}
for i in range(len(q)):
cfg[self.robot.actuated_joints[i].name] = q[i]
return cfg
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 show(self, q=None, obstacles=None):
cfg = self.get_config(q)
# print(cfg)
fk = self.robot.link_fk(cfg=cfg)
scene = pyrender.Scene()
# adding robot to the scene
for tm in fk:
pose = fk[tm]
init_pose, link_mesh = self.get_link_mesh(tm)
mesh = pyrender.Mesh.from_trimesh(link_mesh, smooth=False)
scene.add(mesh, pose=np.matmul(pose, init_pose))
# adding base box to the scene
# table = box([0.5, 0.5, 0.01])
# table.apply_translation([0, 0, -0.1])
cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414)
# nc = pyrender.Node(camera=cam, matrix=np.eye(4))
# scene.add_node(nc)
init_cam_pose = np.eye(4)
init_cam_pose[2, 3] = 2.5
scene.add(cam, pose=init_cam_pose)
# scene.add(pyrender.Mesh.from_trimesh(table))
# adding obstacles to the scene
for ob in obstacles:
scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False))
pyrender.Viewer(scene, use_raymond_lighting=True)
def animate(self, q_traj=None, obstacles=None):
import time
fps = 10.0
cfgs = [self.get_config(q) for q in q_traj]
# Create the scene
fk = self.robot.link_fk(cfg=cfgs[0])
node_map = {}
init_pose_map = {}
scene = pyrender.Scene()
for tm in fk:
pose = fk[tm]
init_pose, link_mesh = self.get_link_mesh(tm)
mesh = pyrender.Mesh.from_trimesh(link_mesh, smooth=False)
node = scene.add(mesh, pose=np.matmul(pose, init_pose))
node_map[tm] = node
init_pose_map[tm] = init_pose
cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414)
init_cam_pose = np.eye(4)
init_cam_pose[2, 3] = 2.5
scene.add(cam, pose=init_cam_pose)
for ob in obstacles:
scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False))
# Pop the visualizer asynchronously
v = pyrender.Viewer(scene, run_in_thread=True,
use_raymond_lighting=True)
time.sleep(1.0)
# Now, run our loop
i = 0
while v.is_active:
cfg = cfgs[i]
fk = self.robot.link_fk(cfg=cfg)
if i < len(cfgs) - 1:
i += 1
else:
i = 0
time.sleep(1.0)
v.render_lock.acquire()
for mesh in fk:
pose = fk[mesh]
node_map[mesh].matrix = np.matmul(pose, init_pose_map[mesh])
v.render_lock.release()
time.sleep(1.0 / fps)