# Path: lfd/environment/sim_util.py
# PR2_L_POSTURES = dict(
#     untucked = [0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
#     tucked = [0.06, 1.25, 1.79, -1.68, -1.73, -0.10, -0.09],
#     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]
# )
#     GRIPPER_L_FINGER_OPEN_CLOSE_THRESH = mult * settings.GRIPPER_OPEN_CLOSE_THRESH
# class RopeState(object):
# class RopeParams(object):
# class SceneState(object):
#     def __init__(self, init_rope_nodes, rope_params, tfs=None):
#     def __init__(self):
#     def __eq__(self, other):
#     def __ne__(self, other):
#     def __init__(self, cloud, rope_nodes, rope_state, id=None, color=None):
#     def get_unique_id():
# def make_table_xml(translation, extents):
# def make_box_xml(name, translation, extents):
# def make_cylinder_xml(name, translation, radius, height):
# def reset_arms_to_side(sim_env):
# def arm_moved(joint_traj):    
# def split_trajectory_by_gripper(seg_info):
# def split_trajectory_by_lr_gripper(seg_info, lr):
# def get_opening_closing_inds(finger_traj):
# def gripper_joint2gripper_l_finger_joint_values(gripper_joint_vals):
# def binarize_gripper(angle):
# def get_binary_gripper_angle(open):
# def set_gripper_maybesim(sim_env, lr, is_open, prev_is_open, animate=False):
# def mirror_arm_joints(x):
# def unwrap_arm_traj_in_place(traj):
# def unwrap_in_place(t, dof_inds=None):
# def dof_inds_from_name(robot, name):
# def sim_traj_maybesim(sim_env, lr2traj, animate=False, interactive=False, max_cart_vel_trans_traj=.05):
# def sim_full_traj_maybesim(sim_env, full_traj, animate=False, interactive=False, max_cart_vel_trans_traj=.05):
#     def sim_callback(i):
#     def is_rope_pulled_too_tight(i_step):
# def get_full_traj(robot, lr2arm_traj, lr2finger_traj = {}):
# def merge_full_trajs(full_trajs):
# def get_ee_traj(robot, lr, arm_traj_or_full_traj, ee_link_name_fmt="%s_gripper_tool_frame"):
# def get_finger_rel_pts(finger_lr):
# def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj):
# def grippers_exceed_rope_length(sim_env, full_traj, thresh):
# def remove_tight_rope_pull(sim_env, full_traj):
# def load_random_start_segment(demofile):
# def load_fake_data_segment(demofile, fake_data_segment, fake_data_transform):
# def unif_resample(traj, max_diff, wt = None):        
# def get_rope_transforms(sim_env):
# def replace_rope(new_rope, sim_env, rope_params, restore=False):
# def set_rope_transforms(tfs, sim_env):
# def get_rope_params(params_id):
# def tpsrpm_plot_cb(sim_env, x_nd, y_md, targ_Nd, corr_nm, wt_n, f):
# def draw_grid(sim_env, old_xyz, f, color = (1,1,0,1)):
# def draw_axis(sim_env, hmat, arrow_length=.1, arrow_width=0.005):
# def draw_finger_pts_traj(sim_env, flr2finger_pts_traj, color):
# 
# 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]


import argparse
import importlib, __builtin__
import util
import openravepy, trajoptpy
import h5py, numpy as np
from lfd.environment import sim_util
from lfd.rapprentice import math_utils as mu
from string import lower
        else:
            v = dataset[()]
    return v

def save_results_args(fname, args):
    # if args is already in the results file, make sure that the eval arguments are the same
    if fname is None:
        return
    result_file = h5py.File(fname, 'a')

    if 'args' in result_file:
        loaded_args = group_or_dataset_to_obj(result_file['args'])
        if 'eval' not in vars(loaded_args):
            raise RuntimeError("The file doesn't have eval arguments")
        if 'eval' not in vars(args):
            raise RuntimeError("The current arguments doesn't have eval arguments")
        loaded_args_eval_dict = vars(loaded_args.eval)
        args_eval_dict = vars(args.eval)
        inconsistent_args = False
        inconsistent_args_msg = ""
        if set(loaded_args_eval_dict.keys()) != set(args_eval_dict.keys()):
            inconsistent_args = True
        for (k, args_eval_val) in args_eval_dict.iteritems():
            if inconsistent_args:
                break
            loaded_args_eval_val = loaded_args_eval_dict[k]
            if np.any(args_eval_val != loaded_args_eval_val):
                inconsistent_args = True
                inconsistent_args_msg = "%s, %s"%(loaded_args_eval_val, args_eval_val)
        if inconsistent_args:

# Path: lfd/environment/sim_util.py
# PR2_L_POSTURES = dict(
#     untucked = [0.4,  1.0,   0.0,  -2.05,  0.0,  -0.1,  0.0],
#     tucked = [0.06, 1.25, 1.79, -1.68, -1.73, -0.10, -0.09],
#     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]
# )
#     GRIPPER_L_FINGER_OPEN_CLOSE_THRESH = mult * settings.GRIPPER_OPEN_CLOSE_THRESH
# class RopeState(object):
# class RopeParams(object):
# class SceneState(object):
#     def __init__(self, init_rope_nodes, rope_params, tfs=None):
#     def __init__(self):
#     def __eq__(self, other):
#     def __ne__(self, other):
#     def __init__(self, cloud, rope_nodes, rope_state, id=None, color=None):
#     def get_unique_id():
# def make_table_xml(translation, extents):
# def make_box_xml(name, translation, extents):
# def make_cylinder_xml(name, translation, radius, height):
# def reset_arms_to_side(sim_env):
# def arm_moved(joint_traj):    
# def split_trajectory_by_gripper(seg_info):
# def split_trajectory_by_lr_gripper(seg_info, lr):
# def get_opening_closing_inds(finger_traj):
# def gripper_joint2gripper_l_finger_joint_values(gripper_joint_vals):
# def binarize_gripper(angle):
# def get_binary_gripper_angle(open):
# def set_gripper_maybesim(sim_env, lr, is_open, prev_is_open, animate=False):
# def mirror_arm_joints(x):
# def unwrap_arm_traj_in_place(traj):
# def unwrap_in_place(t, dof_inds=None):
# def dof_inds_from_name(robot, name):
# def sim_traj_maybesim(sim_env, lr2traj, animate=False, interactive=False, max_cart_vel_trans_traj=.05):
# def sim_full_traj_maybesim(sim_env, full_traj, animate=False, interactive=False, max_cart_vel_trans_traj=.05):
#     def sim_callback(i):
#     def is_rope_pulled_too_tight(i_step):
# def get_full_traj(robot, lr2arm_traj, lr2finger_traj = {}):
# def merge_full_trajs(full_trajs):
# def get_ee_traj(robot, lr, arm_traj_or_full_traj, ee_link_name_fmt="%s_gripper_tool_frame"):
# def get_finger_rel_pts(finger_lr):
# def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj):
# def grippers_exceed_rope_length(sim_env, full_traj, thresh):
# def remove_tight_rope_pull(sim_env, full_traj):
# def load_random_start_segment(demofile):
# def load_fake_data_segment(demofile, fake_data_segment, fake_data_transform):
# def unif_resample(traj, max_diff, wt = None):        
# def get_rope_transforms(sim_env):
# def replace_rope(new_rope, sim_env, rope_params, restore=False):
# def set_rope_transforms(tfs, sim_env):
# def get_rope_params(params_id):
# def tpsrpm_plot_cb(sim_env, x_nd, y_md, targ_Nd, corr_nm, wt_n, f):
# def draw_grid(sim_env, old_xyz, f, color = (1,1,0,1)):
# def draw_axis(sim_env, hmat, arrow_length=.1, arrow_width=0.005):
# def draw_finger_pts_traj(sim_env, flr2finger_pts_traj, color):
# 
# 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/eval_util.py
import argparse
import importlib, __builtin__
import util
import openravepy, trajoptpy
import h5py, numpy as np
from lfd.environment import sim_util
from lfd.rapprentice import math_utils as mu
from string import lower

        else:
            v = dataset[()]
    return v

def save_results_args(fname, args):
    # if args is already in the results file, make sure that the eval arguments are the same
    if fname is None:
        return
    result_file = h5py.File(fname, 'a')

    if 'args' in result_file:
        loaded_args = group_or_dataset_to_obj(result_file['args'])
        if 'eval' not in vars(loaded_args):
            raise RuntimeError("The file doesn't have eval arguments")
        if 'eval' not in vars(args):
            raise RuntimeError("The current arguments doesn't have eval arguments")
        loaded_args_eval_dict = vars(loaded_args.eval)
        args_eval_dict = vars(args.eval)
        inconsistent_args = False
        inconsistent_args_msg = ""
        if set(loaded_args_eval_dict.keys()) != set(args_eval_dict.keys()):
            inconsistent_args = True
        for (k, args_eval_val) in args_eval_dict.iteritems():
            if inconsistent_args:
                break
            loaded_args_eval_val = loaded_args_eval_dict[k]
            if np.any(args_eval_val != loaded_args_eval_val):
                inconsistent_args = True
                inconsistent_args_msg = "%s, %s"%(loaded_args_eval_val, args_eval_val)
        if inconsistent_args:

            user_resp = raw_input("The arguments of the file and the current arguments have different eval arguments %s, overwrite?[y/N]"%inconsistent_args_msg)