from IPython import embed as shell
import itertools
import random

SEED = 1234
NUM_PROBS = 5
WIDTH = 7
HEIGHT = 2
SIZE = 1
NUM_CANS = 4 # each can i starts at target i, so we must have NUM_CANS <= NUM_TARGETS
NUM_TARGETS = 8
GOAL = "(At can0 target1), (At can1 target0)"

CAN_RADIUS=0.3
ROBOT_RADIUS=0.4

BACK = (3.5,6)
FRONT = (3.5, 4)

ROBOT_INIT=(2,2)

def main():
    random.seed(SEED)
    for iteration in range(NUM_PROBS):
        s = "# AUTOGENERATED. DO NOT EDIT.\n# Configuration file for NAMO problem instance. Blank lines and lines beginning with # are filtered out.\n\n"
        coords = list(itertools.product(list(range(WIDTH)), list(range(HEIGHT))))
        random.shuffle(coords)
        coords[0] = list(FRONT)
        coords[1] = list(BACK)
        coord_ind = 0
        s += "# The values after each attribute name are the values that get passed into the __init__ method for that attribute's class defined in the domain configuration.\n"
        s += "Objects: "
        for i in range(NUM_TARGETS):
            s += "Target (name target%d); "%(i)
            s += "RobotPose (name pdp_target%d); "%i
            if i < NUM_CANS:
                s += "Can (name can%d); "%i
                # s += "RobotPose (name gp_can%d); "%i
            s += "Grasp (name grasp{}); ".format(i)
        s += "Robot (name %s); "%"pr2"

        s += "Obstacle (name wall); "
        s += "RobotPose (name %s) \n\n"%"robot_init_pose"


        s += "Init: "
        for i in range(NUM_TARGETS):
            if i < 2:
                s += "(value target{} {}) ".format(i, list(coords[i]))
            else:
                s += "(value target{} undefined) ".format(i, list(coords[i]))
            s += "(geom target{} {}), ".format(i, CAN_RADIUS)
            s += "(value pdp_target%d undefined), "%i
            s += "(value grasp{} undefined), ".format(i)
        for i in range(NUM_CANS):
            s += "(geom can{} {}), (pose can{} {}), ".format(
                i, CAN_RADIUS, i, list(coords[i]))
            # s += "(value gp_can%d undefined), "%i

        s += "(pose wall [0, 0]), (geom wall closet), "
        s += "(geom {} {}), ".format("pr2", ROBOT_RADIUS)
        s += "(pose pr2 {}), ".format(list(ROBOT_INIT))
        s += "(value {} {}); ".format("robot_init_pose", list(ROBOT_INIT))
        for i in range(NUM_CANS):
            s += "(At can{} target{}), ".format(i, i)
            s += "(Stationary can{}), ".format(i)
            for j in range(NUM_CANS):
                s += "(StationaryNEq can{} can{}), ".format(i, j)
            # s += "(InContact pr2 gp_can{} target{}), ".format(i, i)
            # s += "(GraspValid gp_can{} target{} grasp0), ".format(i, i)
        for i in range(NUM_TARGETS):
            s += "(InContact pr2 pdp_target{} target{}), ".format(i, i)
            s += "(GraspValid pdp_target{} target{} grasp{}), ".format(i, i, i)
        s += "(RobotAt pr2 robot_init_pose), "
        s += "(StationaryW wall), "
        s += "(IsMP pr2)\n\n"
        s += "Goal: %s"%GOAL

        with open("namo_probs/swap_%d_%d.prob"%(SEED, iteration), "w") as f:
            f.write(s)

if __name__ == "__main__":
    main()

from IPython import embed as shell
import itertools
import random

SEED = 1234
NUM_PROBS = 5
WIDTH = 7
HEIGHT = 2
SIZE = 1
NUM_CANS = 4 # each can i starts at target i, so we must have NUM_CANS <= NUM_TARGETS
NUM_TARGETS = 8
GOAL = "(At can0 target1), (At can1 target0)"

CAN_RADIUS=0.3
ROBOT_RADIUS=0.4

BACK = (3.5,6)
FRONT = (3.5, 4)

ROBOT_INIT=(2,2)

def main():
    random.seed(SEED)
    for iteration in range(NUM_PROBS):
        s = "# AUTOGENERATED. DO NOT EDIT.\n# Configuration file for NAMO problem instance. Blank lines and lines beginning with # are filtered out.\n\n"
        coords = list(itertools.product(list(range(WIDTH)), list(range(HEIGHT))))
        random.shuffle(coords)
        coords[0] = list(FRONT)
        coords[1] = list(BACK)
        coord_ind = 0
        s += "# The values after each attribute name are the values that get passed into the __init__ method for that attribute's class defined in the domain configuration.\n"
        s += "Objects: "
        for i in range(NUM_TARGETS):
            s += "Target (name target%d); "%(i)
            s += "RobotPose (name pdp_target%d); "%i
            if i < NUM_CANS:
                s += "Can (name can%d); "%i
                # s += "RobotPose (name gp_can%d); "%i
            s += "Grasp (name grasp{}); ".format(i)
        s += "Robot (name %s); "%"pr2"

        s += "Obstacle (name wall); "
        s += "RobotPose (name %s) \n\n"%"robot_init_pose"


        s += "Init: "
        for i in range(NUM_TARGETS):
            if i < 2:
                s += "(value target{} {}) ".format(i, list(coords[i]))
            else:
                s += "(value target{} undefined) ".format(i, list(coords[i]))
            s += "(geom target{} {}), ".format(i, CAN_RADIUS)
            s += "(value pdp_target%d undefined), "%i
            s += "(value grasp{} undefined), ".format(i)
        for i in range(NUM_CANS):
            s += "(geom can{} {}), (pose can{} {}), ".format(
                i, CAN_RADIUS, i, list(coords[i]))
            # s += "(value gp_can%d undefined), "%i

        s += "(pose wall [0, 0]), (geom wall closet), "
        s += "(geom {} {}), ".format("pr2", ROBOT_RADIUS)
        s += "(pose pr2 {}), ".format(list(ROBOT_INIT))
        s += "(value {} {}); ".format("robot_init_pose", list(ROBOT_INIT))
        for i in range(NUM_CANS):
            s += "(At can{} target{}), ".format(i, i)
            s += "(Stationary can{}), ".format(i)
            for j in range(NUM_CANS):
                s += "(StationaryNEq can{} can{}), ".format(i, j)
            # s += "(InContact pr2 gp_can{} target{}), ".format(i, i)
            # s += "(GraspValid gp_can{} target{} grasp0), ".format(i, i)
        for i in range(NUM_TARGETS):
            s += "(InContact pr2 pdp_target{} target{}), ".format(i, i)
            s += "(GraspValid pdp_target{} target{} grasp{}), ".format(i, i, i)
        s += "(RobotAt pr2 robot_init_pose), "
        s += "(StationaryW wall), "
        s += "(IsMP pr2)\n\n"
        s += "Goal: %s"%GOAL

        with open("namo_probs/swap_%d_%d.prob"%(SEED, iteration), "w") as f:
            f.write(s)

if __name__ == "__main__":
    main()

# each can i starts at target i, so we must have NUM_CANS <= NUM_TARGETS # Configuration file for NAMO problem instance. Blank lines and lines beginning with # are filtered out.\n\n" # s += "RobotPose (name gp_can%d); "%i # s += "(value gp_can%d undefined), "%i # s += "(InContact pr2 gp_can{} target{}), ".format(i, i) # s += "(GraspValid gp_can{} target{} grasp0), ".format(i, i)