# Path: stripstream/utils.py
# def flatten(iterable_of_iterables):
#     return (item for iterables in iterable_of_iterables for item in iterables)
# 
# Path: robotics/openrave/transforms.py
# def pose_from_quat_point(quat, point):
#     return np.concatenate([quat, point])
# 
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def unit_quat():
#     return np.array((1, 0, 0, 0))


from inspect import currentframe, getfile
from itertools import product
from random import sample
from stripstream.utils import flatten
from utils import set_base_conf, Pose, box_body, get_name,  set_manipulator_conf, mirror_arm_config, open_gripper, close_gripper, top_grasps, Grasp,  SURFACE_Z_OFFSET, compute_surface
from robotics.openrave.transforms import pose_from_quat_point, set_pose, unit_quat
import os
import math
import numpy as np
    separation = (side_dim, side_dim)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7
    table = box_body(env, 'table', length, width, height, color=TAN)
    set_pose(table, pose_from_quat_point(unit_quat(), np.array([0, 0, 0])))
    env.Add(table)

    robot = env.GetRobots()[0]
    set_manipulator_conf(robot.GetManipulator('leftarm'), TOP_HOLDING_LEFT_ARM)
    open_gripper(robot.GetManipulator('leftarm'))
    set_manipulator_conf(robot.GetManipulator('rightarm'),
                         mirror_arm_config(robot, REST_LEFT_ARM))
    close_gripper(robot.GetManipulator('rightarm'))
    robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
    set_base_conf(robot, (-.75, .2, -math.pi / 2))

    poses = []
    z = height + SURFACE_Z_OFFSET
    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(Pose(pose_from_quat_point(

# Path: stripstream/utils.py
# def flatten(iterable_of_iterables):
#     return (item for iterables in iterable_of_iterables for item in iterables)
# 
# Path: robotics/openrave/transforms.py
# def pose_from_quat_point(quat, point):
#     return np.concatenate([quat, point])
# 
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def unit_quat():
#     return np.array((1, 0, 0, 0))


# Path: robotics/openrave/problems.py
from inspect import currentframe, getfile
from itertools import product
from random import sample
from stripstream.utils import flatten
from utils import set_base_conf, Pose, box_body, get_name,  set_manipulator_conf, mirror_arm_config, open_gripper, close_gripper, top_grasps, Grasp,  SURFACE_Z_OFFSET, compute_surface
from robotics.openrave.transforms import pose_from_quat_point, set_pose, unit_quat
import os
import math
import numpy as np

    separation = (side_dim, side_dim)

    coordinates = list(product(range(m), range(n)))
    assert n_obj <= len(coordinates)
    obj_coordinates = sample(coordinates, n_obj)

    length = m * (box_dims[0] + separation[0])
    width = n * (box_dims[1] + separation[1])
    height = .7
    table = box_body(env, 'table', length, width, height, color=TAN)
    set_pose(table, pose_from_quat_point(unit_quat(), np.array([0, 0, 0])))
    env.Add(table)

    robot = env.GetRobots()[0]
    set_manipulator_conf(robot.GetManipulator('leftarm'), TOP_HOLDING_LEFT_ARM)
    open_gripper(robot.GetManipulator('leftarm'))
    set_manipulator_conf(robot.GetManipulator('rightarm'),
                         mirror_arm_config(robot, REST_LEFT_ARM))
    close_gripper(robot.GetManipulator('rightarm'))
    robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
    set_base_conf(robot, (-.75, .2, -math.pi / 2))

    poses = []
    z = height + SURFACE_Z_OFFSET
    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(Pose(pose_from_quat_point(

                unit_quat(), np.array([x, y, z]))))