def robot_parts_to_node(self,node): parts = [] for part in self.robot_parts: tmat = transform.aboutZ(part.orient) tmat = transform.translate(part.center[0,0], part.center[1,0]).dot(tmat) tmat = transform.aboutZ(node.q).dot(tmat) tmat = transform.translate(node.x, node.y).dot(tmat) this_part = part.instantiate(tmat) parts.append(this_part) return parts
def generate_wall_obstacles(self, wall): wall_spec = wall_marker_dict[wall.id] half_length = wall.length / 2 widths = [] last_x = -half_length edges = [[0, -half_length, 0., 1.]] for (center, width) in wall_spec.doorways: left_edge = center - width / 2 - half_length edges.append([0., left_edge, 0., 1.]) widths.append(left_edge - last_x) right_edge = center + width / 2 - half_length edges.append([0., right_edge, 0., 1.]) last_x = right_edge edges.append([0., half_length, 0., 1.]) widths.append(half_length - last_x) edges = np.array(edges).T edges = transform.aboutZ(wall.theta).dot(edges) edges = transform.translate(wall.x, wall.y).dot(edges) obst = [] for i in range(0, len(widths)): center = edges[:, 2 * i:2 * i + 2].mean(1).reshape(4, 1) dimensions = (4.0, widths[i]) r = Rectangle(center=center, dimensions=dimensions, orient=wall.theta) r.obstacle = wall obst.append(r) return obst
def parts_to_node(self,node): parts = [] for part in self.robot_parts: center = transform.point(part.center[0,0]+node.x, part.center[1,0]+node.y) tmat = transform.translate(center[0,0],center[1,0]) \ .dot(transform.aboutZ(part.orient)) this_part = part.instantiate(tmat) parts.append(this_part) return parts
def __init__(self, center=None, dimensions=None, orient=0): self.center = center self.dimensions = dimensions self.orient = orient dx2 = dimensions[0] / 2 dy2 = dimensions[1] / 2 vertices = np.array([[-dx2, dx2, dx2, -dx2], [-dy2, -dy2, dy2, dy2], [0, 0, 0, 0], [1, 1, 1, 1]]) self.unrot = transform.aboutZ(-orient) center_ex = self.unrot.dot(center) extents = transform.translate(center_ex[0], center_ex[1]).dot(vertices) # Extents measured along the rectangle's axes, not world axes self.min_Ex = min(extents[0, :]) self.max_Ex = max(extents[0, :]) self.min_Ey = min(extents[1, :]) self.max_Ey = max(extents[1, :]) vertices = transform.aboutZ(orient).dot(vertices) vertices = transform.translate(center[0], center[1]).dot(vertices) super().__init__(vertices=vertices)