<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
from typing import List, Tuple, Callable, Dict, Iterable

import SasToGtsam
from SasToGtsam import SASToGTSAM

DiscreteKey = Tuple[int, int]

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/gripper_01.sas"
sas.read_file(sas_dir)
converter = SASToGTSAM(sas)
start = time.time()
k = 13
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)

operators = []
for j in range(k-1):
    op_key = converter.generate_operator_key(j)
    operators.append(op_key)
    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)

# this is where the planning happens
# enum OrderingType { COLAMD, METIS, NATURAL, CUSTOM };
# orderingType  = gtsam.Ordering.OrderingType.METIS
# dag = graph.maxProduct(orderingType)


ordering = gtsam.Ordering()
# From Initial State
# for j in range(k):
#     for state in states[j]:
#         ordering.push_back(state[0])
#     if j < k-1:
#         ordering.push_back(operators[j][0])

# From Goal State
for j in reversed(range(k)):
    if j < k-1:
        ordering.push_back(operators[j][0])
    for state in states[j]:
        ordering.push_back(state[0])

# Mixed 1/3
# for i in range(k//3):
#     for state in states[i]:
#         ordering.push_back(state[0])
#     if i < k-1:
#         ordering.push_back(operators[i][0])

# for j in range(k-1, k//3-1, -1):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
#     for state in states[j]:
#         ordering.push_back(state[0])

# Mixed 1/2
# for i in range(k//2):
#     for state in states[i]:
#         ordering.push_back(state[0])
#     if i < k-1:
#         ordering.push_back(operators[i][0])

# for j in range(k-1, k//2-1, -1):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
#     for state in states[j]:
#         ordering.push_back(state[0])

# Mixed 2/3
# for i in range(2*k//3):
#     for state in states[i]:
#         ordering.push_back(state[0])
#     if i < k-1:
#         ordering.push_back(operators[i][0])

# for j in range(k-1, 2*k//3-1, -1):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
#     for state in states[j]:
#         ordering.push_back(state[0])

# for j in reversed(range(k)):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
    
# for j in reversed(range(k)):
#     for state in states[j]:
#         ordering.push_back(state[0])

# minigraph = gtsam.DiscreteFactorGraph()

dag = graph.maxProduct(ordering)
result = dag.argmax()

# result = graph.optimize()

end = time.time()
# assert graph(result) == 1
print("time taken:", end - start)
op_consts = []
for i in range(dag.size()-1, dag.size()-k, -1):
    op_consts.append(dag.at(i))
val_list = []
for op_const in reversed(op_consts):
    print(converter.ops_names[result[op_const.operatorKey()]])
def plan(plan_length):
    for k in range(18, plan_length):
        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)

        operators = []
        for j in range(k-1):
            op_key = converter.generate_operator_key(j)
            operators.append(op_key)
            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)

        # this is where the planning happens
        # enum OrderingType { COLAMD, METIS, NATURAL, CUSTOM };
        # orderingType  = gtsam.Ordering.OrderingType.METIS
        # dag = graph.maxProduct(orderingType)


        ordering = gtsam.Ordering()
        # initial to goal
        # for j in range(k):
        #     for state in states[j]:
        #         ordering.push_back(state[0])
        #     if j < k-1:
        #         ordering.push_back(operators[j][0])

        # goal to initial
        for j in reversed(range(k)):
            if j < k-1:
                ordering.push_back(operators[j][0])
            for state in states[j]:
                ordering.push_back(state[0])

        dag = graph.maxProduct(ordering)
        result = dag.argmax()

        if graph(result) == 0.0:
            print(k)
            del graph
            continue
        else:
            return graph, result, k
    return "longer plan length?", "no result", plan_length
start = time.time()
graph, result, k = plan(30)
end = time.time()
assert graph(result) == 1
print("time taken:", end - start)
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[result[op_const.operatorKey()]])
graph.print()
start = time.time()
k = 2
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)

operators = []
for j in range(k-1):
    op_key = converter.generate_operator_key(j)
    operators.append(op_key)
    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)

# this is where the planning happens
# enum OrderingType { COLAMD, METIS, NATURAL, CUSTOM };
# orderingType  = gtsam.Ordering.OrderingType.METIS
# dag = graph.maxProduct(orderingType)


ordering = gtsam.Ordering()
# initial to goal
for j in range(k):
    for state in states[j]:
        ordering.push_back(state[0])
    if j < k-1:
        ordering.push_back(operators[j][0])

# goal to initial
# for j in reversed(range(k)):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
#     for state in states[j]:
#         ordering.push_back(state[0])

for j in reversed(range(k)):
    if j < k-1:
        ordering.push_back(operators[j][0])
    
for j in reversed(range(k)):
    for state in states[j]:
        ordering.push_back(state[0])

minigraph = gtsam.DiscreteFactorGraph()

dag = graph.maxProduct(ordering)
result = dag.argmax()

# result = graph.optimize()

end = time.time()
assert graph(result) == 1
print("time taken:", end - start)
start = time.time()
k = 2
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)

operators = []
for j in range(k-1):
    op_key = converter.generate_operator_key(j)
    operators.append(op_key)
    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)

# this is where the planning happens
# enum OrderingType { COLAMD, METIS, NATURAL, CUSTOM };
# orderingType  = gtsam.Ordering.OrderingType.METIS
# dag = graph.maxProduct(orderingType)


ordering = gtsam.Ordering()
# for j in range(k):
#     for state in states[j]:
#         ordering.push_back(state[0])
#     if j < k-1:
#         ordering.push_back(operators[j][0])

# for j in reversed(range(k)):
#     if j < k-1:
#         ordering.push_back(operators[j][0])
#     for state in states[j]:
#         ordering.push_back(state[0])

for j in reversed(range(k)):
    if j < k-1:
        ordering.push_back(operators[j][0])
    
for j in reversed(range(k)):
    for state in states[j]:
        ordering.push_back(state[0])

minigraph = gtsam.DiscreteFactorGraph()

dag = graph.maxProduct(ordering)
result = dag.argmax()

# result = graph.optimize()

end = time.time()
assert graph(result) == 1
print("time taken:", end - start)
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.1170789163722026, 'fraction_numerical': 0.009422850412249705, 'mean_word_length': 3.3751288217107525, 'pattern_counts': {'":': 0, '<': 21, '<?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': '230026', 'license': 'BSD-2-Clause', 'n_tokens_mistral': 4520, 'n_tokens_neox': 4334, 'n_words': 991, 'path': 'python/Planning_gripper.ipynb', 'repo_name': 'ywkim0606/gtsam_python', 'size': '12735'}