# 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
    
    traj[0] = transition_traj[-1]
    unwrap_in_place(traj, dof_inds=dof_inds)
    traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj) # make the trajectory slow enough for the simulation

    valid_inds = grippers_exceed_rope_length(sim_env, (traj, dof_inds), 0.05)
    min_gripper_dist = [np.inf] # minimum distance between gripper when the rope capsules are too far apart
    def is_rope_pulled_too_tight(i_step):
        if valid_inds is None or valid_inds[i_step]: # gripper is not holding the rope or the grippers are not that far apart
            return True
        rope = sim_env.sim.rope
        trans = rope.GetTranslations()
        hhs = rope.GetHalfHeights()
        rots = rope.GetRotations()
        fwd_pts = (trans + hhs[:,None]*rots[:,:3,0])
        bkwd_pts = (trans - hhs[:,None]*rots[:,:3,0])
        pts_dists = np.apply_along_axis(np.linalg.norm, 1, fwd_pts[:-1] - bkwd_pts[1:])[:,None] # these should all be zero if the rope constraints are satisfied
        if np.any(pts_dists > sim_env.sim.rope_params.radius):
            if i_step == 0:
                return True
            ee_trajs = {}
            for lr in 'lr':
                ee_trajs[lr] = get_ee_traj(sim_env, lr, (traj[i_step-1:i_step+1], dof_inds), ee_link_name_fmt="%s_gripper_l_finger_tip_link")
            min_gripper_dist[0] = min(min_gripper_dist[0], np.linalg.norm(ee_trajs['r'][0,:3,3] - ee_trajs['l'][0,:3,3]))
            grippers_moved_closer = np.linalg.norm(ee_trajs['r'][1,:3,3] - ee_trajs['l'][1,:3,3]) < min_gripper_dist[0]
            return grippers_moved_closer
        return True
    animate_traj.animate_traj(traj, sim_env.robot, restore=False, pause=interactive,
        callback=sim_callback, step_viewer=animate_speed, execute_step_cond=is_rope_pulled_too_tight)
    if min_gripper_dist[0] != np.inf:

# 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

    
    traj[0] = transition_traj[-1]
    unwrap_in_place(traj, dof_inds=dof_inds)
    traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj) # make the trajectory slow enough for the simulation

    valid_inds = grippers_exceed_rope_length(sim_env, (traj, dof_inds), 0.05)
    min_gripper_dist = [np.inf] # minimum distance between gripper when the rope capsules are too far apart
    def is_rope_pulled_too_tight(i_step):
        if valid_inds is None or valid_inds[i_step]: # gripper is not holding the rope or the grippers are not that far apart
            return True
        rope = sim_env.sim.rope
        trans = rope.GetTranslations()
        hhs = rope.GetHalfHeights()
        rots = rope.GetRotations()
        fwd_pts = (trans + hhs[:,None]*rots[:,:3,0])
        bkwd_pts = (trans - hhs[:,None]*rots[:,:3,0])
        pts_dists = np.apply_along_axis(np.linalg.norm, 1, fwd_pts[:-1] - bkwd_pts[1:])[:,None] # these should all be zero if the rope constraints are satisfied
        if np.any(pts_dists > sim_env.sim.rope_params.radius):
            if i_step == 0:
                return True
            ee_trajs = {}
            for lr in 'lr':
                ee_trajs[lr] = get_ee_traj(sim_env, lr, (traj[i_step-1:i_step+1], dof_inds), ee_link_name_fmt="%s_gripper_l_finger_tip_link")
            min_gripper_dist[0] = min(min_gripper_dist[0], np.linalg.norm(ee_trajs['r'][0,:3,3] - ee_trajs['l'][0,:3,3]))
            grippers_moved_closer = np.linalg.norm(ee_trajs['r'][1,:3,3] - ee_trajs['l'][1,:3,3]) < min_gripper_dist[0]
            return grippers_moved_closer
        return True
    animate_traj.animate_traj(traj, sim_env.robot, restore=False, pause=interactive,
        callback=sim_callback, step_viewer=animate_speed, execute_step_cond=is_rope_pulled_too_tight)
    if min_gripper_dist[0] != np.inf:

        util.yellowprint("Some steps of the trajectory were not executed because the gripper was pulling the rope too tight.")