# Path: lfd/rapprentice/animate_traj.py
# def animate_traj(traj, robot, pause=True, step_viewer=1, restore=True, callback=None, execute_step_cond=None):
#     """make sure to set active DOFs beforehand"""
#     if restore: _saver = openravepy.RobotStateSaver(robot)
#     if step_viewer or pause: viewer = trajoptpy.GetViewer(robot.GetEnv())
#     for (i,dofs) in enumerate(traj):
#         sys.stdout.write("step %i/%i\r"%(i+1,len(traj)))
#         sys.stdout.flush()
#         if callback is not None: callback(i)
#         if execute_step_cond is not None and not execute_step_cond(i): continue
#         robot.SetActiveDOFValues(dofs)
#         if pause: viewer.Idle()
#         elif step_viewer!=0 and (i%step_viewer==0 or i==len(traj)-1): viewer.Step()
#     sys.stdout.write("\n")
# 
# Path: lfd/rapprentice/ropesim.py
# def transform(hmat, p):
# def in_grasp_region(robot, lr, pt):
#     def on_inner_side(pt, finger_lr):
# def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
# def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
#     def __init__(self, env, robot, rope_params):
#     def create(self, rope_pts):
#     def __del__(self):
#     def step(self):
#     def settle(self, max_steps=100, tol=.001, animate=False):
#     def observe_cloud(self, upsample=0, upsample_rad=1):
#     def raycast_cloud(self, T_w_k=None, obj=None, z=1., endpoints=0):
#     def grab_rope(self, lr):
#     def release_rope(self, lr):
#     def is_grabbing_rope(self, lr):
# class Simulation(object):


import openravepy
import trajoptpy
import bulletsimpy
import numpy as np
import sim_util
import settings
import importlib
from lfd.rapprentice import animate_traj, ropesim
from robot_world import RobotWorld
                from lfd.rapprentice import berkeley_pr2
            self._include_gripper_finger_collisions()
        n_robots = len(self.env.GetRobots())
        for obj_to_add in objs_to_add:
            if obj_to_add.dynamic:
                raise RuntimeError("Dynamic object can't be added to StaticSimulation")
            else:
                self.sim_objs.append(obj_to_add)
                obj_to_add.add_to_env(self)
        if len(self.env.GetRobots()) > n_robots:
            if self.robot:
                raise NotImplementedError("Behavior for adding more than one robot has not been defined")
            self.robot = self.env.GetRobots()[-1]
        if consider_finger_collisions:
            self._exclude_gripper_finger_collisions()
    
    def remove_objects(self, objs_to_remove, consider_finger_collisions=True):
        if consider_finger_collisions:
            self._include_gripper_finger_collisions()
        for obj_to_remove in objs_to_remove:
            if obj_to_remove.dynamic:
                raise RuntimeError("Dynamic object can't be removed from StaticSimulation")
            else:
                self.sim_objs.remove(obj_to_remove)
                obj_to_remove.remove_from_env()
        if self.robot and self.robot not in self.env.GetRobots():
            raise NotImplementedError("Behavior for removing robots has not been defined")
        if consider_finger_collisions:
            self._exclude_gripper_finger_collisions()
    
    def get_state(self):

# Path: lfd/rapprentice/animate_traj.py
# def animate_traj(traj, robot, pause=True, step_viewer=1, restore=True, callback=None, execute_step_cond=None):
#     """make sure to set active DOFs beforehand"""
#     if restore: _saver = openravepy.RobotStateSaver(robot)
#     if step_viewer or pause: viewer = trajoptpy.GetViewer(robot.GetEnv())
#     for (i,dofs) in enumerate(traj):
#         sys.stdout.write("step %i/%i\r"%(i+1,len(traj)))
#         sys.stdout.flush()
#         if callback is not None: callback(i)
#         if execute_step_cond is not None and not execute_step_cond(i): continue
#         robot.SetActiveDOFValues(dofs)
#         if pause: viewer.Idle()
#         elif step_viewer!=0 and (i%step_viewer==0 or i==len(traj)-1): viewer.Step()
#     sys.stdout.write("\n")
# 
# Path: lfd/rapprentice/ropesim.py
# def transform(hmat, p):
# def in_grasp_region(robot, lr, pt):
#     def on_inner_side(pt, finger_lr):
# def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
# def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
#     def __init__(self, env, robot, rope_params):
#     def create(self, rope_pts):
#     def __del__(self):
#     def step(self):
#     def settle(self, max_steps=100, tol=.001, animate=False):
#     def observe_cloud(self, upsample=0, upsample_rad=1):
#     def raycast_cloud(self, T_w_k=None, obj=None, z=1., endpoints=0):
#     def grab_rope(self, lr):
#     def release_rope(self, lr):
#     def is_grabbing_rope(self, lr):
# class Simulation(object):


# Path: lfd/environment/simulation.py
import openravepy
import trajoptpy
import bulletsimpy
import numpy as np
import sim_util
import settings
import importlib
from lfd.rapprentice import animate_traj, ropesim
from robot_world import RobotWorld
                from lfd.rapprentice import berkeley_pr2

            self._include_gripper_finger_collisions()
        n_robots = len(self.env.GetRobots())
        for obj_to_add in objs_to_add:
            if obj_to_add.dynamic:
                raise RuntimeError("Dynamic object can't be added to StaticSimulation")
            else:
                self.sim_objs.append(obj_to_add)
                obj_to_add.add_to_env(self)
        if len(self.env.GetRobots()) > n_robots:
            if self.robot:
                raise NotImplementedError("Behavior for adding more than one robot has not been defined")
            self.robot = self.env.GetRobots()[-1]
        if consider_finger_collisions:
            self._exclude_gripper_finger_collisions()
    
    def remove_objects(self, objs_to_remove, consider_finger_collisions=True):
        if consider_finger_collisions:
            self._include_gripper_finger_collisions()
        for obj_to_remove in objs_to_remove:
            if obj_to_remove.dynamic:
                raise RuntimeError("Dynamic object can't be removed from StaticSimulation")
            else:
                self.sim_objs.remove(obj_to_remove)
                obj_to_remove.remove_from_env()
        if self.robot and self.robot not in self.env.GetRobots():
            raise NotImplementedError("Behavior for removing robots has not been defined")
        if consider_finger_collisions:
            self._exclude_gripper_finger_collisions()
    
    def get_state(self):

        sim_objs_constructor_infos = [sim_obj._get_constructor_info() for sim_obj in self.sim_objs]