def get_path_state(self, robot_state: RobotState) -> PathState: """Given the current pose of the robot and an optional lookahead distance, get a `PathState` object describing the optimal state. """ closest, closest_segment_index = self._find_closest_point( robot_state.position) lookahead = self._compute_lookahead(closest, closest_segment_index) absolute_goal = self._find_goal_point(robot_state, lookahead) or closest relative_goal = utils.vehicle_coords(robot_state, absolute_goal) D = utils.distance_between(Point(), relative_goal) x = relative_goal.x curvature = self.curvature_scaling * ((2 * x) / (D * D)) remaining_distance = utils.distance_between(robot_state.position, self.end) return PathState(curvature=curvature, goal_point=absolute_goal, path_position=closest, goal_direction=utils.signum(relative_goal.y, separate_zero=False), remaining_distance=remaining_distance)
def test_distance_between(): p1 = utils.Point(0.0, 0.0) p2 = utils.Point(1.0, 1.0) p3 = utils.Point(-1.0, -1.0) p4 = utils.Point(-4.0, 3.0) assert utils.distance_between(p1, p2) == pytest.approx(math.sqrt(2.0)) assert utils.distance_between(p1, p3) == pytest.approx(math.sqrt(2.0)) assert utils.distance_between(p4, p2) == pytest.approx(math.sqrt(29.0)) assert utils.distance_between(p4, p3) == pytest.approx(5.0)
def __init__(self, waypoints: typing.List[Waypoint], end_angle: float): """Create a `Path` following along between the `waypoints` `end_angle`: Desired ending angle. The robot may not end up facing this direction at all, but will be biased to point this direction. The longer the lookahead of the last waypoint, the more likely it is that the robot will end up oriented in the desired direction. `waypoints` must contained at least two waypoints - a start and an end. Less than that will result in undefined behavior. """ self.segments = [] for i, waypoint in enumerate(waypoints): start = waypoint.position try: end = waypoints[i + 1].position except IndexError: last = waypoints[-1].position end_length = (1 + 1e-6) * waypoints[-1].lookahead end = Point(x=last.x + (math.cos(end_angle) * end_length), y=last.y + (math.sin(end_angle) * end_length)) length = utils.distance_between(start, end) angle = utils.angle_between(start, end) segment = PathSegment( start=start, end=end, length=length, angle=angle, start_lookahead=waypoint.lookahead, start_curvature_scaling=waypoint.curvature_scaling) self.segments.append(segment)
def _find_closest_point(self, point: Point) -> (Point, int): """Find the closest point on the path to the given point Returns the closest point, and the index of the path segment it is on.""" candidates = [] for i, segment in enumerate(self.segments): candidate = utils.closest_point_on_line(segment, point) distance = utils.distance_between(candidate, point) candidates.append((candidate, distance, i)) closest = min(candidates, key=lambda x: x[1]) return closest[0], closest[2]
def _compute_lookahead(self, path_position: Point, segment_index: int) -> float: segment = self.segments[segment_index] start_lookahead = segment.start_lookahead try: end_lookahead = self.segments[segment_index + 1].start_lookahead except IndexError: end_lookahead = start_lookahead distance_to_end = utils.distance_between(path_position, segment.end) lookahead = utils.interpolate(end_lookahead, start_lookahead, 0, segment.length, distance_to_end) return lookahead
def __init__( self, tuning_parameters: PathTuning, initial_robot_state: RobotState, waypoints: typing.List[Point], ): """Path starting from `initial_robot_state` following between the `waypoints` Tuning parameters: `lookahead` will be used to select a goal point, during the last segment of the path, it will be linearly scaled from `lookahead` to `lookahead / lookahead_reduction_factor` across that last segment. `curvature_scaling` should be used to tune how responsive the robot is to turning - if the robot fails to complete curves, increase it, if it turns too sharply decrease this value. """ self.initial_state = initial_robot_state self.lookahead = tuning_parameters.lookahead self.lookahead_reduction = tuning_parameters.lookahead_reduction_factor self.curvature_scaling = tuning_parameters.curvature_scaling position = self.initial_state.position self.segments = [] for waypoint in waypoints: length = utils.distance_between(position, waypoint) angle = utils.angle_between(position, waypoint) segment = PathSegment(start=position, end=waypoint, length=length, angle=angle) self.segments.append(segment) position = waypoint self.end = self.segments[-1].end end_stabilization_length = self.lookahead * 1.1 end_angle = self.segments[-1].angle self.pseudo_end = Point( x=self.end.x + (math.cos(end_angle) * end_stabilization_length), y=self.end.y + (math.sin(end_angle) * end_stabilization_length)) self.segments.append( PathSegment(start=self.end, end=self.pseudo_end, length=end_stabilization_length, angle=end_angle))
def _compute_curvature_scaling(self, path_position: Point, segment_index: int) -> float: segment = self.segments[segment_index] start_curve_scaling = segment.start_curvature_scaling try: end_curve_scaling = self.segments[segment_index + 1].start_curvature_scaling except IndexError: end_curve_scaling = start_curve_scaling distance_to_end = utils.distance_between(path_position, segment.end) curvature_scaling = utils.interpolate(end_curve_scaling, start_curve_scaling, 0, segment.length, distance_to_end) return curvature_scaling
def _compute_lookahead(self, path_position: Point, segment_index: int) -> float: segment = self.segments[segment_index] on_end_segment = segment_index == len(self.segments) - 2 on_pseudo_segment = segment_index == len(self.segments) - 1 if on_pseudo_segment: return self.lookahead / self.lookahead_reduction if on_end_segment: remaining_distance = utils.distance_between( path_position, self.end) scale = ((segment.length + (self.lookahead_reduction - 1) * remaining_distance) / (self.lookahead_reduction * segment.length)) return self.lookahead * scale return self.lookahead
def _find_goal_point(self, robot_state: RobotState, lookahead: float) -> Point: """Find a goal point - point on path `lookahead` from robot that is closest by on-path distance to the end Returns `None` if no such intersection is found. """ for segment in reversed(self.segments): intersections = utils.circle_line_intersection( robot_state.position, lookahead, segment) intersections = [ n for n in intersections if utils.on_segment(n, segment) ] if intersections: return min( intersections, key=(lambda x: utils.distance_between(x, segment.end))) return None