# 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
    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]))))
        poses.append(row)

    initial_poses = {}
    goal_poses = {}
    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
        initial_poses[name] = poses[r][c]
        obj = box_body(env, name, *box_dims, color=color)
        set_pose(obj, poses[r][c].value)
        env.Add(obj)


# 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

    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]))))
        poses.append(row)

    initial_poses = {}
    goal_poses = {}
    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
        initial_poses[name] = poses[r][c]
        obj = box_body(env, name, *box_dims, color=color)
        set_pose(obj, poses[r][c].value)
        env.Add(obj)


    known_poses = list(flatten(poses))