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)
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)
init_ds_cells(folder_RES_STL, ic_GT=False)
init_ds_cells(input_folder_GT, ic_GT=True)
print("# Part2b: already compose excel with the RES geometric data")
write_excel_RES_data()
# Part3 : find max contacting volume for each GT cell (always start from GT cells = similar to SEG scoring ).
# 1 : Use the collision detection to detect which cells overlap.
# 2 : check the boolean difference volume.
# 3 : pick the maximum for scoring
print('# Part3 : Checking collision pairs via trimesh...')
is_collision, s_t_colliding_pairs = cm_GT.in_collision_other(cm_RES, return_names=True, return_data=False)
if not is_collision:
print("No collision between the cells, exitting")
exit()
if verbose:print(s_t_colliding_pairs)
for t_colliding_pair in s_t_colliding_pairs:
# print('--Checking collision pair -> {0}'.format(t_colliding_pair))
name_cell_1, name_cell_2 = t_colliding_pair
cell_1 = Cell.d_name_cell[name_cell_1]
cell_2 = Cell.d_name_cell[name_cell_2]
print("start calculating intersection : {0}".format(t_colliding_pair),flush=True)
trimesh_intersection = cell_1.trimesh.intersection(cell_2.trimesh)
# diff.export(str(mesh_path.with_name(mesh_path.stem + '_no_overlap.stl'))) #export to same folder