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)
