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)