<jupyter_start><jupyter_text><jupyter_code>%pip install -q -U gtbook
import numpy as np
from numpy.random import default_rng
rng = default_rng()
import plotly.express as px
try:
    import google.colab
except:
    import plotly.io as pio
    pio.renderers.default = "png"

import gtsam 
from gtbook import logistics
from gtbook.display import show, pretty<jupyter_output><empty_output><jupyter_text>Planning for Logistics> We will be planning in a discritized 2D state space, just like we did for the Markov localization state space.**This Section is still in draft mode and was released for adventurous spirits (and TAs) only.**<jupyter_code>from gtbook.display import randomImages
from IPython.display import display
display(randomImages(4, 5, "steampunk", 1))<jupyter_output><empty_output><jupyter_text>Value Iteration in 2D> It works, but can be slow. Recall that value iteration starts with an initial guess, $V^0$, and at each iteration we updateour approximation of the value function by the update rule:$$V^{k+1}(x) \leftarrow \max_a \left\{ \bar{R}(x,a) + \gamma \sum_{x'} P(x'|x, a) V^k(x')   \right\}= \max_a Q(x, a; V^k)$$This can be *very* expensive if applied naively, when the number of states is large. Indeed, if we use the 5000-state finite element discretization of the warehouse, the conditional probability table (CPT) above contains $5k \times 4 \times 5k = 100M$ entries.There are ways to deal with this, computationally. Indeed, most transitions have zero probability, and hence we could store the CPT in a *sparse* multi-dimensional array instead. Alternatively, we can implement the Q value calculation as a *function*. This is what we will do below, and we will also immediately switch to using the 2D coordinates $(i,j)$ for the discretized cells:<jupyter_code>goal = 35, 50
proximity = logistics.base_map + logistics.proximity_map_on

def Q_value(x, a, V):
    """Given a state x=(i,j) action a, and value image, calculate Q value."""
    i, j = x
    if x==goal: return 0 # only reward when *first* arriving at goal
    if logistics.base_map[i,j]: return 0 # invalid state
    if a == 0: next = (i,j-1) if j>0 else x # LEFT
    if a == 1: next = (i,j+1) if j<99 else x # RIGHT
    if a == 2: next = (i-1,j) if i>0 else x # UP
    if a == 3: next = (i+1,j) if i<49 else x # DOWN
    reward = 100 if next==goal else 0
    r, c = next
    if logistics.proximity_map_on[i,j]: reward -= 50 # in proximity is bad!
    if logistics.proximity_map_on[r,c]: reward -= 50 # as is going in proximity.
    return reward + 0.975 * V[r,c]

def update_V(x, V):
    """Given a state x=(i,j) and value image, calculate updated value."""
    Q_x = [Q_value(x, a, V) for a in range(4)]
    return max(Q_x)<jupyter_output><empty_output><jupyter_text>Note above we assumed actions were *deterministic*, but the function can be easily adapted to non-deterministic actions. We awarded 100 points upon reaching the goal, but make sure that happens only once, or otherwise the value of that state will be infinite. In addition, when the state is near an obstacle (we use the `proximity_map_on` likelihood image from Section 4.3), the agent gets punished by 100 points. Below, we do value iteration using a brute force loop, which is still remarkably fast:<jupyter_code>%%time
I, J = np.meshgrid(range(50), range(100), indexing='ij')
states = np.dstack((I, J)).reshape(5000,2)
k = 0
V_k = np.full((50,100), 0.0)
error = float('inf')
while error>1e-5:
    V_plus = np.empty((50,100), float)
    for i,j in states:
        V_plus[i,j] = update_V((i,j), V_k)
    error = np.max(np.abs(V_plus-V_k))
    V_k = V_plus
    k += 1
print(f"after {k} iterations, error = {error}")
px.imshow(V_k, color_continuous_scale='Turbo',origin="upper", aspect='equal')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.11028828352287755, 'fraction_numerical': 0.0224808251785242, 'mean_word_length': 4.356940509915014, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '520985', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 1328, 'n_tokens_neox': 1237, 'n_words': 492, 'path': 'S45_logistics_planning.ipynb', 'repo_name': 'gtbook/robotics', 'size': '3781'}