U9.9 Planning for a car

The green bay is on the other side of the wall, a 30 cm square round the goal, and the car must finish in it facing back down the mat (180 degrees, within 25). Driving one plan to the end misses it, as above. Write plan(), an A* search over the car's three moves, then a loop that plans from where the robot really is, drives only the first move with car(), and plans again. Stop when the plan comes back empty, and say so if it comes back with no path at all: those are different endings. No sliding, no turning on the spot, no touching the wall, and no list of moves typed in by hand. free(), arc(), here() and car() are written for you.

from bugbot import *
import heapq, math
connect()

V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0
SPEED, STEP_S = 60, 1.6                    # every move: 60 percent for 1.6 s, about 19 cm on paper
INFLATE = 9.0
START = (50.0, 40.0)
GOAL, GOAL_H = (155.0, 40.0), 180.0        # the far side of the wall, facing back down the mat
WALLS = [(95.0, 0.0, 10.0, 120.0)]
V_SEEN = SPEED / 100 * V_MAX               # the speed the model uses; car() replaces it with the one it measures

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def arc(x, y, h, steer, seconds, n=8):
    """Where car(SPEED, steer, seconds) goes at V_SEEN on a circle of R_MIN, as n points along the way."""
    v = V_SEEN
    w = math.degrees(v / R_MIN) * steer
    pts = []
    for k in range(n):
        dt = seconds / n
        a, wr = math.radians(h), math.radians(w)
        if abs(wr) < 1e-9:
            C, S = math.cos(a) * dt, math.sin(a) * dt
        else:
            C = (math.sin(a + wr * dt) - math.sin(a)) / wr
            S = (math.cos(a) - math.cos(a + wr * dt)) / wr
        x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
        pts.append((x, y, h))
    return pts

def here():
    px, py = position()
    return START[0] + px, START[1] + py, heading()

def car(speed, steer, seconds, dt=0.1):
    """Drive at speed, steer -1..1, and every dt set the turn rate from the speed measured, so the radius stays R_MIN."""
    global V_SEEN
    speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer))       # top speed and full lock, as in U2.8
    rot = 100 * math.degrees(speed / 100 * V_MAX / R_MIN) * steer / W_MAX      # first guess, from the nominal speed
    for k in range(round(seconds / dt)):
        (x0, y0), h0 = position(), heading()
        drive(speed, 0, rot)
        wait(dt)
        (x1, y1), h1 = position(), heading()
        v = math.copysign(math.hypot(x1 - x0, y1 - y0) / dt, speed)
        w = ((h1 - h0 + 180) % 360 - 180) / dt
        rot += 0.5 * (math.degrees(v / R_MIN) * steer - w) * 100 / W_MAX
        if k >= 4:                                                            # once it is up to speed
            V_SEEN += 0.2 * (abs(v) - V_SEEN)

# write plan(): A* over the car's three moves

# then the loop: plan from here(), drive only the first move, and plan again

The hint students can ask for: Two jobs. First the search: work out what a state is, what its three neighbours are (use arc() and keep only moves whose points are all free()), what a move costs, a heuristic that never overestimates, and what counts as arriving (within 8 cm of the goal and within 25 degrees of 180). The plan() in the lesson does all of this. Then the loop: each time round, plan from here(), drive only the first move, and think about the two ways plan() can come back with no move to drive: already there, or no way from here. They need different endings.

A solution

from bugbot import *
import heapq, math
connect()

V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0
SPEED, STEP_S = 60, 1.6                    # every move: 60 percent for 1.6 s, about 19 cm on paper
INFLATE = 9.0
START = (50.0, 40.0)
GOAL, GOAL_H = (155.0, 40.0), 180.0        # the far side of the wall, facing back down the mat
WALLS = [(95.0, 0.0, 10.0, 120.0)]
V_SEEN = SPEED / 100 * V_MAX               # the speed the model uses; car() replaces it with the one it measures

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def arc(x, y, h, steer, seconds, n=8):
    """Where car(SPEED, steer, seconds) goes at V_SEEN on a circle of R_MIN, as n points along the way."""
    v = V_SEEN
    w = math.degrees(v / R_MIN) * steer
    pts = []
    for k in range(n):
        dt = seconds / n
        a, wr = math.radians(h), math.radians(w)
        if abs(wr) < 1e-9:
            C, S = math.cos(a) * dt, math.sin(a) * dt
        else:
            C = (math.sin(a + wr * dt) - math.sin(a)) / wr
            S = (math.cos(a) - math.cos(a + wr * dt)) / wr
        x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
        pts.append((x, y, h))
    return pts

def here():
    px, py = position()
    return START[0] + px, START[1] + py, heading()

def car(speed, steer, seconds, dt=0.1):
    """Drive at speed, steer -1..1, and every dt set the turn rate from the speed measured, so the radius stays R_MIN."""
    global V_SEEN
    speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer))       # top speed and full lock, as in U2.8
    rot = 100 * math.degrees(speed / 100 * V_MAX / R_MIN) * steer / W_MAX      # first guess, from the nominal speed
    for k in range(round(seconds / dt)):
        (x0, y0), h0 = position(), heading()
        drive(speed, 0, rot)
        wait(dt)
        (x1, y1), h1 = position(), heading()
        v = math.copysign(math.hypot(x1 - x0, y1 - y0) / dt, speed)
        w = ((h1 - h0 + 180) % 360 - 180) / dt
        rot += 0.5 * (math.degrees(v / R_MIN) * steer - w) * 100 / W_MAX
        if k >= 4:                                                            # once it is up to speed
            V_SEEN += 0.2 * (abs(v) - V_SEEN)

def plan(sx, sy, sh):
    """A* over the car's three moves: the steers from (sx, sy, sh) to the goal, [] if already there, None if no way."""
    def cell(x, y, h):
        return (round(x / 6), round(y / 6), round(h / 45) % 8)
    def cost_to_go(x, y):
        return max(0.0, math.hypot(GOAL[0] - x, GOAL[1] - y) - 8)      # the goal counts from 8 cm away
    q = [(cost_to_go(sx, sy), 0.0, (sx, sy, sh), [])]
    seen = set()
    while q:
        f, g, (x, y, h), moves = heapq.heappop(q)
        if math.hypot(GOAL[0] - x, GOAL[1] - y) < 8 and abs((h - GOAL_H + 180) % 360 - 180) < 25:
            return moves
        c = cell(x, y, h)
        if c in seen:
            continue
        seen.add(c)
        for steer in (0, -1, 1):
            pts = arc(x, y, h, steer, STEP_S)
            if all(free(px, py) for px, py, _ in pts):
                nx, ny, nh = pts[-1]
                step = V_SEEN * STEP_S * (1.0 if steer == 0 else 1.15)          # turning costs a little more
                heapq.heappush(q, (g + step + cost_to_go(nx, ny), g + step, (nx, ny, nh), moves + [steer]))
    return None

# receding horizon: plan from where the robot really is, drive only the first move, and plan again
for i in range(60):
    moves = plan(*here())
    if moves is None:
        print("no path from here")
        break
    if not moves:
        break
    print("plan", i + 1, "has", len(moves), "moves")
    car(SPEED, moves[0], STEP_S)
stop()
x, y, h = here()
print("ended at", round(x), round(y), "facing", round(h), "after", i, "moves")

Any program that meets the task's checks is marked correct in the simulator; this is one way, not the only way.