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 _get_cylinder(radius, normal, center, height=0.1, sections=10):
"""Get a thin disk at a given location and orientation."""
rotation = rotation_matrix(
angle=_angle_between([0, 0, 1], normal),
direction=np.cross([0, 0, 1], normal),
)
cyl = cylinder(radius=radius,
height=height,
transform=rotation,
sections=sections)
cyl.vertices += center
return cyl
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 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 __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)
