# Path: lfd/rapprentice/eval_util.py
# class EvalStats(object):
#     def __init__(self, **kwargs):
# def get_indexed_items(itemsfile, task_list=None, task_file=None, i_start=-1, i_end=-1):
# def add_obj_to_group(group, k, v):
# def group_or_dataset_to_obj(group_or_dataset):
# def save_results_args(fname, args):
# def load_results_args(fname):
# def save_task_results_step(fname, task_index, step_index, results):
# def load_task_results_step(fname, task_index, step_index):
# def traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
# def traj_is_safe(sim_env, full_traj, collision_dist_threshold, upsample=0):
# 
# Path: lfd/demonstration/demonstration.py
# class Demonstration(object):
# class SceneState(object):
# class GroundTruthRopeSceneState(SceneState):
# class AugmentedTrajectory(object):
#     def __init__(self, name, scene_state, aug_traj):
#     def __repr__(self):
#     def __init__(self, full_cloud, id=None, full_color=None, downsample_size=0):
#     def get_unique_id():
#     def __repr__(self):
#     def __init__(self, rope_nodes, radius, upsample=0, upsample_rad=1, downsample_size=0):
#     def __init__(self, lr2arm_traj=None, lr2finger_traj=None, lr2ee_traj=None, lr2open_finger_traj=None, lr2close_finger_traj=None):
#     def __eq__(self, other):
#     def __ne__(self, other):
#     def create_from_full_traj(robot, full_traj, lr2open_finger_traj=None, lr2close_finger_traj=None):
#     def get_full_traj(self, robot):
#     def get_resampled_traj(self, timesteps_rs):
#     def __repr__(self):
# 
# Path: lfd/environment/simulation_object.py
# class SimulationObject(object):
# class XmlSimulationObject(SimulationObject):
# class BoxSimulationObject(XmlSimulationObject):
# class CylinderSimulationObject(XmlSimulationObject):
# class RopeSimulationObject(SimulationObject):
#     def __init__(self, names, dynamic=True):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def get_bullet_objects(self):
#     def get_state(self):
#     def set_state(self, tfs):
#     def _get_constructor_info(self):
#     def __init__(self, xml, dynamic=False):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, translation, extents, dynamic=True):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, translation, radius, height, dynamic=True):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, ctrl_points, rope_params=None, dynamic=True, upsample=0, upsample_rad=1):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def get_bullet_objects(self):
#     def get_state(self):
#     def set_state(self, tfs):
#     def _get_constructor_info(self):
#     def __repr__(self):


import numpy as np
from lfd.rapprentice import eval_util
from lfd.demonstration import demonstration
from lfd.environment import simulation_object
                        self.world.close_gripper(lr, step_viewer=step_viewer)
                        if len(self.sim.constraints[lr]) == n_cnts and lr == 'l':
                            misgraspl = True
                        elif lr == 'l':
                            misgraspl = False
                        elif len(self.sim.constraints[lr]) == n_cnts and lr=='r':
                            misgraspr = True
                        else:
                            misgraspr = False
                        #misgrasp |= len(self.sim.constraints[lr]) == n_cnts
                        lr2gripper_open[lr] = False
            misgrasp = misgraspl or misgraspr
            # don't execute trajectory for finger joint if the corresponding gripper is closed
            active_inds = np.ones(len(dof_inds), dtype=bool)
            for lr in 'lr':
                if not lr2gripper_open[lr]:
                    joint_ind = self.sim.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
                    if joint_ind in dof_inds:
                        active_inds[dof_inds.index(joint_ind)] = False
            miniseg_traj = traj[start_ind:end_ind, active_inds]
            miniseg_dof_inds = list(np.asarray(dof_inds)[active_inds])
            full_traj = (miniseg_traj, miniseg_dof_inds)
            feasible &= eval_util.traj_is_safe(self.sim, full_traj, 0)
            if check_feasible and not feasible:
                break
            self.world.execute_trajectory(full_traj, step_viewer=step_viewer, interactive=interactive, sim_callback=sim_callback)
        return feasible, misgrasp
    
    def observe_scene(self):
        full_cloud = self.world.observe_cloud()

# Path: lfd/rapprentice/eval_util.py
# class EvalStats(object):
#     def __init__(self, **kwargs):
# def get_indexed_items(itemsfile, task_list=None, task_file=None, i_start=-1, i_end=-1):
# def add_obj_to_group(group, k, v):
# def group_or_dataset_to_obj(group_or_dataset):
# def save_results_args(fname, args):
# def load_results_args(fname):
# def save_task_results_step(fname, task_index, step_index, results):
# def load_task_results_step(fname, task_index, step_index):
# def traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
# def traj_is_safe(sim_env, full_traj, collision_dist_threshold, upsample=0):
# 
# Path: lfd/demonstration/demonstration.py
# class Demonstration(object):
# class SceneState(object):
# class GroundTruthRopeSceneState(SceneState):
# class AugmentedTrajectory(object):
#     def __init__(self, name, scene_state, aug_traj):
#     def __repr__(self):
#     def __init__(self, full_cloud, id=None, full_color=None, downsample_size=0):
#     def get_unique_id():
#     def __repr__(self):
#     def __init__(self, rope_nodes, radius, upsample=0, upsample_rad=1, downsample_size=0):
#     def __init__(self, lr2arm_traj=None, lr2finger_traj=None, lr2ee_traj=None, lr2open_finger_traj=None, lr2close_finger_traj=None):
#     def __eq__(self, other):
#     def __ne__(self, other):
#     def create_from_full_traj(robot, full_traj, lr2open_finger_traj=None, lr2close_finger_traj=None):
#     def get_full_traj(self, robot):
#     def get_resampled_traj(self, timesteps_rs):
#     def __repr__(self):
# 
# Path: lfd/environment/simulation_object.py
# class SimulationObject(object):
# class XmlSimulationObject(SimulationObject):
# class BoxSimulationObject(XmlSimulationObject):
# class CylinderSimulationObject(XmlSimulationObject):
# class RopeSimulationObject(SimulationObject):
#     def __init__(self, names, dynamic=True):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def get_bullet_objects(self):
#     def get_state(self):
#     def set_state(self, tfs):
#     def _get_constructor_info(self):
#     def __init__(self, xml, dynamic=False):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, translation, extents, dynamic=True):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, translation, radius, height, dynamic=True):
#     def _get_constructor_info(self):
#     def __repr__(self):
#     def __init__(self, name, ctrl_points, rope_params=None, dynamic=True, upsample=0, upsample_rad=1):
#     def add_to_env(self, sim):
#     def remove_from_env(self):
#     def get_bullet_objects(self):
#     def get_state(self):
#     def set_state(self, tfs):
#     def _get_constructor_info(self):
#     def __repr__(self):


# Path: lfd/environment/environment.py
import numpy as np
from lfd.rapprentice import eval_util
from lfd.demonstration import demonstration
from lfd.environment import simulation_object

                        self.world.close_gripper(lr, step_viewer=step_viewer)
                        if len(self.sim.constraints[lr]) == n_cnts and lr == 'l':
                            misgraspl = True
                        elif lr == 'l':
                            misgraspl = False
                        elif len(self.sim.constraints[lr]) == n_cnts and lr=='r':
                            misgraspr = True
                        else:
                            misgraspr = False
                        #misgrasp |= len(self.sim.constraints[lr]) == n_cnts
                        lr2gripper_open[lr] = False
            misgrasp = misgraspl or misgraspr
            # don't execute trajectory for finger joint if the corresponding gripper is closed
            active_inds = np.ones(len(dof_inds), dtype=bool)
            for lr in 'lr':
                if not lr2gripper_open[lr]:
                    joint_ind = self.sim.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
                    if joint_ind in dof_inds:
                        active_inds[dof_inds.index(joint_ind)] = False
            miniseg_traj = traj[start_ind:end_ind, active_inds]
            miniseg_dof_inds = list(np.asarray(dof_inds)[active_inds])
            full_traj = (miniseg_traj, miniseg_dof_inds)
            feasible &= eval_util.traj_is_safe(self.sim, full_traj, 0)
            if check_feasible and not feasible:
                break
            self.world.execute_trajectory(full_traj, step_viewer=step_viewer, interactive=interactive, sim_callback=sim_callback)
        return feasible, misgrasp
    
    def observe_scene(self):
        full_cloud = self.world.observe_cloud()

        return demonstration.SceneState(full_cloud, downsample_size=self.downsample_size)