# 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
        self.goal_regions = goal_regions
        self.known_poses = known_poses
        self.initial_poses = initial_poses
        self.known_grasps = known_grasps

    def __str__(self):
        s = self.__class__.__name__ + '\n'
        for name, value in self.__dict__.iteritems():
            s += name + ': %s\n' % str(value)
        return s
    __repr__ = __str__


def dantam_distract(env, n_obj):
    env.Load(os.path.join(ENVIRONMENTS_DIR, 'empty.xml'))

    m, n = 3, 3
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    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)

# 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

        self.goal_regions = goal_regions
        self.known_poses = known_poses
        self.initial_poses = initial_poses
        self.known_grasps = known_grasps

    def __str__(self):
        s = self.__class__.__name__ + '\n'
        for name, value in self.__dict__.iteritems():
            s += name + ': %s\n' % str(value)
        return s
    __repr__ = __str__


def dantam_distract(env, n_obj):
    env.Load(os.path.join(ENVIRONMENTS_DIR, 'empty.xml'))

    m, n = 3, 3
    side_dim = .07
    height_dim = .1
    box_dims = (side_dim, side_dim, height_dim)
    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])))