class DeltaNode(Node): """ Node for the graph in the Delta space, which is a 2D space where axis 1 is the first path and axis 2 is the second path. Units are in meters, representing the distance from the start of the path. """ def __init__(self, x, y): Node.__init__(self) self.__pos = Point(x, y) @property def x(self): return self.__pos.x @property def y(self): return self.__pos.y def position(self): return self.__pos def dist(self, arg): """ Returns the distance to a certain object. :param arg: Object to calculate distance. Can be Point, Pose, StablePose or MeshNode. :return: float, distance to the object. """ if isinstance(arg, DeltaNode): return self.__pos.dist(arg.position()) elif isinstance(arg, Point): return self.__pos.dist(arg) else: raise ValueError( "DeltaNode can't calculate distances to object of type " + type(arg).__name__) def __eq__(self, node): """ Checks if two nodes are equal. :param node: """ return self.dist(node) < EPS def __hash__(self): """ Hash function to use in dicts. :return: HashCode associated to Node. """ return self.x.__hash__() ^ self.y.__hash__() def __repr__(self): """ Used for printing node. :return: String representing MeshNode. """ return "[" + str(self.x) + ", " + str(self.y) + "]"
def __repr__(self): s = "Poses: [" for i in range(len(self.__poses) - 1): s += Point(self.__poses[i].position()).__repr__() + " -> " s += Point(self.__poses[-1].position()).__repr__() + "]" s += "\nTimes: " for i in range(len(self.__times) - 1): s += str(self.__times[i]) + " -> " s += str(self.__times[-1]) + "]" return s
def add_pose(self, pose, time=None): """ Adds a new point to the path. :param pose: Pose of the drone. :param time: Time the drone should reach this pose, in respect to the initial pose in the path. """ self.__poses.append(pose) if time is not None: self.__times.append(time) else: if len(self.__poses) == 1: self.__times.append(0) self.__lengths.append(0) else: duration = self.__poses[-1].dist(self.__poses[-2]) / MAX_VEL self.__times.append(self.__times[-1] + duration) self.__lengths.append(self.__lengths[-1] + Point(self.__poses[-2].position()).dist( Point(self.__poses[-1].position())))
def intersection_2d(self, s): """ Finds the intersection with another segment. Only works for 2d segments. :param s: Segment. :return: Point or None, representing the intersection. """ num = ((s.a.x - self.a.x) * (s.b.y - s.a.y) - (s.a.y - self.a.y) * (s.b.x - s.a.x)) den = ((self.b.x - self.a.x) * (s.b.y - s.a.y) - (self.b.y - self.a.y) * (s.b.x - s.a.x)) if abs(den) > EPS: i = num / den pt = Point(self.a.x + (self.b.x - self.a.x) * i, self.a.y + (self.b.y - self.a.y) * i) if self.contains(pt) and s.contains(pt): return pt else: return None
def add_node(n): for node in self.__nodes: # Only short edges that are not on the ground or through obstacles if n.dist(node) < MESH_EDGE_DIST and (n.z > DRONE_HEIGHT or node.z > DRONE_HEIGHT): valid_edge = True for obs in obstacle_collection.obstacles: if isinstance(obs, Cylinder): cylinder_seg = Segment(Point(obs.position), Point(obs.position)) if obs.axis == 'x': cylinder_seg.b += Point(1000, 0, 0) elif obs.axis == 'y': cylinder_seg.b += Point(0, 1000, 0) else: cylinder_seg.b += Point(0, 0, 1000) edge_seg = Segment(Point(n.position()), Point(node.position())) if cylinder_seg.min_distance( edge_seg) < obs.radius: valid_edge = False if valid_edge: n.add_edge(node)
def convert_to_segments(path): s = [] for i in range(len(path.poses) - 1): s.append(Segment(Point(path.poses[i].position()), Point(path.poses[i + 1].position()))) return s
def __compute_orientations(self, intersections, l1, l2, ids): """ Computes orientations (CW or CCW) for every intersection. It does this by running A* on the 2D space of the intersections, in which each intersection generates 4 nodes. O(i^3), where i is the number of intersections. TODO: This function only works for monotone paths. :param intersections: List of Intersection objects. :param l1: Total length of the first path. :param l2: Total length of the second path. """ # Generating nodes nodes = [DeltaNode(0, 0), DeltaNode(l1, l2), DeltaNode(l1, 0), DeltaNode(0, l2)] for i in intersections: for a in range(2): for b in range(2): nodes.append(DeltaNode(i.interval_1[a], i.interval_2[b])) # For every two nodes, adding edge if it does not pass through any intersection (they can # have a point in common, though) for n1 in nodes: for n2 in nodes: if n1 != n2 and not n1.has_edge(n2): intersect = False for i in intersections: s = Segment(n1.position(), n2.position()) p1 = Point(i.interval_1[0], i.interval_2[0]) p2 = Point(i.interval_1[0], i.interval_2[1]) p3 = Point(i.interval_1[1], i.interval_2[0]) p4 = Point(i.interval_1[1], i.interval_2[1]) s1 = Segment(p1, p2) s2 = Segment(p1, p3) s3 = Segment(p2, p4) s4 = Segment(p3, p4) # If edge intersects intersection, but not on extremity intersect |= s.intersects(s1) and not s.intersects_at_extremity(s1) intersect |= s.intersects(s2) and not s.intersects_at_extremity(s2) intersect |= s.intersects(s3) and not s.intersects_at_extremity(s3) intersect |= s.intersects(s4) and not s.intersects_at_extremity(s4) # Case where it passes through the diagonal intersect |= s.contains(p1) and s.contains(p4) intersect |= s.contains(p2) and s.contains(p3) if not intersect: n1.add_edge(n2) # Changing the distance function. This function only allows monotonous paths, and considers # only the highest distance one of the two drones. This distance will be proportional to # time, because both drones are moving at the same time, and the one that has to fly the # furthest is the one that will take most time. # TODO: This function only allows monotone paths. def dist_function(node1, node2): if node1.x > node2.x + EPS or node1.y > node2.y + EPS: return float("inf") return max(abs(node1.x - node2.x), abs(node1.y - node2.y)) self.__delta_path[ids] = AStarPlanner().plan(nodes[0], nodes[1], dist_function) # Checking sides, for each intersection for i in intersections: clock_wise = False for k in range(len(self.__delta_path[ids]) - 1): s = Segment(self.__delta_path[ids][k].position(), self.__delta_path[ids][k + 1].position()) # Using middle of the intersection p = Point((i.interval_1[0] + i.interval_1[1]) / 2.0, (i.interval_2[0] + i.interval_2[1]) / 2.0) # Tracing a ray up. If it intersects the path, then the point is ccw. # TODO: This only works for monotone paths!! ray = s.intersection_with_line_2d(p, Point(0, 1)) if ray is not None: clock_wise = True break if clock_wise: i.orientation = 1 else: i.orientation = -1
def __init__(self, x, y): Node.__init__(self) self.__pos = Point(x, y)
def random_point(): return Point(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1))
def __event_dt(self, x, x_goal, v): """ Computes the time until the next event. O(n_drones + i), where i is the total number of intersections. :param x: List of np.array of size n_drones and dtype float. Contains, for each step, the position for each drone in the coordination space. :param x_goal: np.array of size n_drones and dtype float. Contains, for each drone, the final position in the coordination space. :param v: np.array of size n_drones and dtype float. Contains a maximal velocity for each drone in the coordination space. :return: float, Time until the next event. """ dt = float("inf") # Time to reach the goals for i in range(self.__n_drones): if v[i] > EPS: dt = min(dt, (x_goal[i] - x[-1][i]) / v[i]) # For each intersection for i in range(self.__n_drones): for j in range(self.__n_drones): if j <= i: continue for intersection in self.__delta[self.__i_to_drone_id[i], self.__i_to_drone_id[j]]: # Tracing ray in direction (v[i], v[j]) pt = Point(x[-1][i], x[-1][j]) vec = Point(v[i], v[j]) if intersection.orientation == 1: seg = Segment( Point(intersection.interval_1[0], 0), Point(intersection.interval_1[0], intersection.interval_2[1])) else: seg = Segment( Point(0, intersection.interval_2[0]), Point(intersection.interval_1[1], intersection.interval_2[0])) # One drone is stopped, the event is at the end of the segment if seg.contains(pt): # If it is at the end of the segment, ignore if not (seg.a == pt or seg.b == pt): if intersection.orientation == 1: dt = min(dt, (intersection.interval_2[1] - pt.y) / v[j]) else: dt = min(dt, (intersection.interval_1[1] - pt.x) / v[i]) # If it reached the segment elif seg.intersection_with_line_2d(pt, vec): if intersection.orientation == 1: dt = min(dt, (intersection.interval_1[0] - pt.x) / v[i]) else: dt = min(dt, (intersection.interval_2[0] - pt.y) / v[j]) if dt < EPS: raise ValueError( "Something went wrong. Next event is current event") return dt