# 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
    table = get_box_body(length, width, height, name='table', color='brown')
    initial_poses.append((table, hu.Pose(0, 0, height / 2, 0)))

    poses = []
    z = height + height_dim / 2 + BODY_PLACEMENT_Z_OFFSET
    theta = 0
    for r in range(m):
        row = []
        x = -length / 2 + (r + .5) * (box_dims[0] + separation[0])
        for c in range(n):
            y = -width / 2 + (c + .5) * (box_dims[1] + separation[1])
            row.append(hu.Pose(x, y, z, theta))
        poses.append(row)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)
    movable_names = []
    for i, (r, c) in enumerate(obj_coordinates):
        row_color = np.zeros(4)
        row_color[2 - r] = 1.
        if i == 0:
            name = 'goal%d-%d' % (r, c)
            color = 'blue'

            goal_poses[name] = poses[m / 2][n / 2]
        else:
            name = 'block%d-%d' % (r, c)
            color = 'red'


# 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

    table = get_box_body(length, width, height, name='table', color='brown')
    initial_poses.append((table, hu.Pose(0, 0, height / 2, 0)))

    poses = []
    z = height + height_dim / 2 + BODY_PLACEMENT_Z_OFFSET
    theta = 0
    for r in range(m):
        row = []
        x = -length / 2 + (r + .5) * (box_dims[0] + separation[0])
        for c in range(n):
            y = -width / 2 + (c + .5) * (box_dims[1] + separation[1])
            row.append(hu.Pose(x, y, z, theta))
        poses.append(row)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)
    movable_names = []
    for i, (r, c) in enumerate(obj_coordinates):
        row_color = np.zeros(4)
        row_color[2 - r] = 1.
        if i == 0:
            name = 'goal%d-%d' % (r, c)
            color = 'blue'

            goal_poses[name] = poses[m / 2][n / 2]
        else:
            name = 'block%d-%d' % (r, c)
            color = 'red'


        obj = get_box_body(*box_dims, name=name, color=color)