# Path: robotics/pyr2/utils.py
# def get_box_body(length, width, height, name='box', color='black'):
#     return Shape([
#         BoxAligned(np.array([(-length / 2., -width / 2., -height / 2),
#                              (length / 2., width / 2., height / 2)]), None)
#     ], None, name=name, color=color)


from itertools import product
from random import sample
from PyR2 import hu
from robotics.pyr2.utils import get_box_body
from PyR2.pr2.pr2Robot import makeRobot
import math
import numpy as np
        self.initial_poses = initial_poses
        self.movable_names = movable_names
        self.goal_conf = goal_conf
        self.goal_poses = goal_poses
        self.known_poses = known_poses


def dantam2():
    m, n = 3, 3

    n_obj = 2
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    separation = (side_dim, side_dim)

    workspace = np.array([[-2.0, -1.5, 0.0],
                          [1.0, 1.5, 2.0]])
    robot = makeRobot(workspace, useLeft=True, useRight=False)
    base_conf = (-.75, .2, -math.pi / 2)
    initial_conf = robot.makeConf(
        *base_conf, g=robot.gripMax, ignoreStandard=True)
    initial_conf = initial_conf.set('pr2LeftArm', TOP_HOLDING_LEFT_ARM)

    initial_poses = []
    goal_poses = {}

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7

# Path: robotics/pyr2/utils.py
# def get_box_body(length, width, height, name='box', color='black'):
#     return Shape([
#         BoxAligned(np.array([(-length / 2., -width / 2., -height / 2),
#                              (length / 2., width / 2., height / 2)]), None)
#     ], None, name=name, color=color)


# Path: robotics/pyr2/problems.py
from itertools import product
from random import sample
from PyR2 import hu
from robotics.pyr2.utils import get_box_body
from PyR2.pr2.pr2Robot import makeRobot
import math
import numpy as np

        self.initial_poses = initial_poses
        self.movable_names = movable_names
        self.goal_conf = goal_conf
        self.goal_poses = goal_poses
        self.known_poses = known_poses


def dantam2():
    m, n = 3, 3

    n_obj = 2
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    separation = (side_dim, side_dim)

    workspace = np.array([[-2.0, -1.5, 0.0],
                          [1.0, 1.5, 2.0]])
    robot = makeRobot(workspace, useLeft=True, useRight=False)
    base_conf = (-.75, .2, -math.pi / 2)
    initial_conf = robot.makeConf(
        *base_conf, g=robot.gripMax, ignoreStandard=True)
    initial_conf = initial_conf.set('pr2LeftArm', TOP_HOLDING_LEFT_ARM)

    initial_poses = []
    goal_poses = {}

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7

    table = get_box_body(length, width, height, name='table', color='brown')