# 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/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]
# 
# Path: lfd/rapprentice/plotting_openrave.py
# def draw_grid(env, f, mins, maxes, xres = .1, yres = .1, zres = .04, color = (1,1,0,1)):
# 
# Path: lfd/util/util.py
# def redprint(msg):
# def yellowprint(msg):
#     def parse_args(self, *args, **kw):
#     def __init__(self, adict):
#     def __init__(self):
#     def __enter__(self):
#     def __exit__(self, *_):
# class ArgumentParser(argparse.ArgumentParser):
# class Bunch(object):
# class suppress_stdout(object):


import bulletsimpy
import openravepy
import numpy as np
import re
import random
import settings
    import scipy.interpolate as si
from numpy import asarray
from lfd.rapprentice import animate_traj, ropesim, math_utils as mu, plotting_openrave
from lfd.util import util
    up = [ 0.33, -0.35,  2.59, -0.15,  0.59, -1.41, -0.27],
    side = [  1.832,  -0.332,   1.011,  -1.437,   1.1  ,  -2.106,  3.074]
)

class RopeState(object):
    def __init__(self, init_rope_nodes, rope_params, tfs=None):
        self.init_rope_nodes = init_rope_nodes
        self.rope_params = rope_params
        self.tfs = tfs

class RopeParams(object):
    def __init__(self):
        self.radius       = settings.ROPE_RADIUS
        self.angStiffness = settings.ROPE_ANG_STIFFNESS
        self.angDamping   = settings.ROPE_ANG_DAMPING
        self.linDamping   = settings.ROPE_LIN_DAMPING
        self.angLimit     = settings.ROPE_ANG_LIMIT
        self.linStopErp   = settings.ROPE_LIN_STOP_ERP
        self.mass         = settings.ROPE_MASS

    def __eq__(self, other):
        if isinstance(other, self.__class__):
            return self.__dict__ == other.__dict__
        else:
            return False
    
    def __ne__(self, other):
        return not self.__eq__(other)

class SceneState(object):

# 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/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]
# 
# Path: lfd/rapprentice/plotting_openrave.py
# def draw_grid(env, f, mins, maxes, xres = .1, yres = .1, zres = .04, color = (1,1,0,1)):
# 
# Path: lfd/util/util.py
# def redprint(msg):
# def yellowprint(msg):
#     def parse_args(self, *args, **kw):
#     def __init__(self, adict):
#     def __init__(self):
#     def __enter__(self):
#     def __exit__(self, *_):
# class ArgumentParser(argparse.ArgumentParser):
# class Bunch(object):
# class suppress_stdout(object):


# Path: lfd/environment/sim_util.py
import bulletsimpy
import openravepy
import numpy as np
import re
import random
import settings
    import scipy.interpolate as si
from numpy import asarray
from lfd.rapprentice import animate_traj, ropesim, math_utils as mu, plotting_openrave
from lfd.util import util

    up = [ 0.33, -0.35,  2.59, -0.15,  0.59, -1.41, -0.27],
    side = [  1.832,  -0.332,   1.011,  -1.437,   1.1  ,  -2.106,  3.074]
)

class RopeState(object):
    def __init__(self, init_rope_nodes, rope_params, tfs=None):
        self.init_rope_nodes = init_rope_nodes
        self.rope_params = rope_params
        self.tfs = tfs

class RopeParams(object):
    def __init__(self):
        self.radius       = settings.ROPE_RADIUS
        self.angStiffness = settings.ROPE_ANG_STIFFNESS
        self.angDamping   = settings.ROPE_ANG_DAMPING
        self.linDamping   = settings.ROPE_LIN_DAMPING
        self.angLimit     = settings.ROPE_ANG_LIMIT
        self.linStopErp   = settings.ROPE_LIN_STOP_ERP
        self.mass         = settings.ROPE_MASS

    def __eq__(self, other):
        if isinstance(other, self.__class__):
            return self.__dict__ == other.__dict__
        else:
            return False
    
    def __ne__(self, other):
        return not self.__eq__(other)

class SceneState(object):

    ids = set()