# 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
    def _include_gripper_finger_collisions(self):
        if not self.robot:
            return
        cc = trajoptpy.GetCollisionChecker(self.env)
        for lr in 'lr':
            for flr in 'lr':
                finger_link_name = "%s_gripper_%s_finger_tip_link" % (lr, flr)
                finger_link = self.robot.GetLink(finger_link_name)
                for sim_obj in self.sim_objs:
                    for bt_obj in sim_obj.get_bullet_objects():
                        for link in bt_obj.GetKinBody().GetLinks():
                            cc.IncludeCollisionPair(finger_link, link)

    @staticmethod
    def simulation_state_equal(s0, s1):
        if s0[0] != s1[0]:
            return False
        d0 = s0[1]
        d1 = s1[1]
        if not set(d0.keys()) == set(d1.keys()):
            return False
        for (k, v0) in d0.iteritems():
            v1 = d1[k]
            if not np.all(v0 == v1):
                return False
        return True


class DynamicSimulation(StaticSimulation):
    def __init__(self, env=None):

# 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

    def _include_gripper_finger_collisions(self):
        if not self.robot:
            return
        cc = trajoptpy.GetCollisionChecker(self.env)
        for lr in 'lr':
            for flr in 'lr':
                finger_link_name = "%s_gripper_%s_finger_tip_link" % (lr, flr)
                finger_link = self.robot.GetLink(finger_link_name)
                for sim_obj in self.sim_objs:
                    for bt_obj in sim_obj.get_bullet_objects():
                        for link in bt_obj.GetKinBody().GetLinks():
                            cc.IncludeCollisionPair(finger_link, link)

    @staticmethod
    def simulation_state_equal(s0, s1):
        if s0[0] != s1[0]:
            return False
        d0 = s0[1]
        d1 = s1[1]
        if not set(d0.keys()) == set(d1.keys()):
            return False
        for (k, v0) in d0.iteritems():
            v1 = d1[k]
            if not np.all(v0 == v1):
                return False
        return True


class DynamicSimulation(StaticSimulation):
    def __init__(self, env=None):

        super(DynamicSimulation, self).__init__(env=env)