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)
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
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)