# 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
        s = self.__class__.__name__ + '\n'
        for name, value in self.__dict__.iteritems():
            s += name + ': %s\n' % str(value)
        return s
    __repr__ = __str__


def simple(env):
    env.Load(os.path.join(ENVIRONMENTS_DIR, '2tables.xml'))

    fixed_names = map(get_name, env.GetBodies())
    tables = filter(lambda body: 'table' in get_name(body), env.GetBodies())
    surfaces = map(compute_surface, tables)
    surface_map = {s.name: s for s in surfaces}

    robot = env.GetRobots()[0]
    set_manipulator_conf(robot.GetManipulator('leftarm'), REST_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, (0, 0, 0))
    robot.SetAffineTranslationLimits(*(2 * np.array([[-1, -1, 0], [1, 1, 0]])))

    objA = box_body(env, 'objA', .07, .05, .2, color=BLUE)
    env.Add(objA)
    while True:
        pose = surface_map['table1'].sample_placement(objA)
        if pose is not None:

# 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

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


def simple(env):
    env.Load(os.path.join(ENVIRONMENTS_DIR, '2tables.xml'))

    fixed_names = map(get_name, env.GetBodies())
    tables = filter(lambda body: 'table' in get_name(body), env.GetBodies())
    surfaces = map(compute_surface, tables)
    surface_map = {s.name: s for s in surfaces}

    robot = env.GetRobots()[0]
    set_manipulator_conf(robot.GetManipulator('leftarm'), REST_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, (0, 0, 0))
    robot.SetAffineTranslationLimits(*(2 * np.array([[-1, -1, 0], [1, 1, 0]])))

    objA = box_body(env, 'objA', .07, .05, .2, color=BLUE)
    env.Add(objA)
    while True:
        pose = surface_map['table1'].sample_placement(objA)
        if pose is not None:

            set_pose(objA, pose)