<jupyter_start><jupyter_text>Planning Notebook<jupyter_code>import math
import unittest
import numpy as np
from itertools import product
import tqdm
from tqdm import tqdm_notebook
import copy
import pickle
import time

# import gtsam
import gtsam
from gtsam import *
from gtsam.utils.test_case import GtsamTestCase

# import gtbook
import gtbook
from gtbook.display import *
from gtbook.discrete import *

# import local package
import gtsam_planner
from gtsam_planner import *

# import parser
import SASParser
from SASParser import SAS, Operator

import SasToGtsam
from SasToGtsam import SASToGTSAM

variables = Variables()
def pretty(obj): 
    return gtbook.display.pretty(obj, variables)

import graphviz
class show(graphviz.Source):
    """ Display an object with a dot method as a graph."""

    def __init__(self, obj):
        """Construct from object with 'dot' method."""
        # This small class takes an object, calls its dot function, and uses the
        # resulting string to initialize a graphviz.Source instance. This in turn
        # has a _repr_mimebundle_ method, which then renders it in the notebook.
        super().__init__(obj.dot())
sas = SAS()
sas_dir = "sas/block_example.sas"
sas.read_file(sas_dir)
converter = SASToGTSAM(sas)
# def plan(plan_length):
#     for k in range(2, plan_length):
#         print(k)
#         states = []
#         mutex_factors = []
#         op_factors = []

#         for i in range(k):
#             # generate state
#             state_t = converter.generate_state(i)
#             states.append(state_t)
#             # generate mutex factor for the state
#             mutex_factor_t = converter.generate_mutex_factor(state_t)
#             mutex_factors.append(mutex_factor_t)
#         for j in range(len(states)-1):
#             op_key = converter.generate_operator_key(j)
#             op_factor = converter.generate_op_factor(states[j], states[j+1], op_key)
#             op_factors.append(op_factor)
#         initial_factor = converter.generate_initial_factor(states[0])
#         goal_factor = converter.generate_goal_factor(states[-1])

#         graph = gtsam.DiscreteFactorGraph()
#         for m_factor in mutex_factors:
#             for f in m_factor:
#                 graph.push_back(f)

#         graph.push_back(goal_factor)
#         graph.push_back(initial_factor)

#         for op_factor in op_factors:
#             graph.push_back(op_factor)

#         val = graph.optimize()
#         if graph(val) == 0.0:
#             del graph
#             continue
#         else:
#             return graph, val, k
#     return "longer plan length?"
start = time.time()
k = 7
states = []
mutex_factors = []
op_factors = []
frame_factors = []
for i in range(k):
    # generate state
    state_t = converter.generate_state(i)
    states.append(state_t)
    # generate mutex factor for the state
    mutex_factor_t = converter.generate_mutex_factor(state_t)
    mutex_factors.append(mutex_factor_t)
for j in range(len(states)-1):
    op_key = converter.generate_operator_key(j)
    op_factor, frame_factor = converter.generate_frame_op_factor(states[j], states[j+1], op_key)
    op_factors.append(op_factor)
    frame_factors.append(frame_factor)
initial_factor = converter.generate_initial_factor(states[0])
goal_factor = converter.generate_goal_factor(states[-1])

graph = gtsam.DiscreteFactorGraph()
for m_factor in mutex_factors:
    for f in m_factor:
        graph.push_back(f)

graph.push_back(goal_factor)
graph.push_back(initial_factor)

for op_factor in op_factors:
    graph.push_back(op_factor)

for frame_factor in frame_factors:
    graph.push_back(frame_factor)

val = graph.optimize()
end = time.time()
assert graph(val) == 1
print("time taken:", end - start)
# graph, val, k = plan(12)
print(graph)
graph(val)
op_consts = []
for i in range(graph.size()-1, graph.size()-k, -1):
    op_consts.append(graph.at(i))
val_list = []
for op_const in reversed(op_consts):
    print(converter.ops_names[val[op_const.operatorKey()]])<jupyter_output>pick-up b
stack b a
pick-up c
stack c b
pick-up d
stack d c
{'fraction_non_alphanumeric': 0.0979960899315738, 'fraction_numerical': 0.004154447702834799, 'mean_word_length': 3.2858638743455497, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 4, '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': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '243994', 'license': 'BSD-2-Clause', 'n_tokens_mistral': 1387, 'n_tokens_neox': 1335, 'n_words': 331, 'path': 'python/Planning_box.ipynb', 'repo_name': 'ywkim0606/gtsam_python', 'size': '4092'}