<s> from __future__ import division <EOL> import time <EOL> import os . path <EOL> import h5py <EOL> import atexit <EOL> import trajoptpy <EOL> import numpy as np <EOL> from lfd . environment import sim_util <EOL> from lfd . environment import settings <EOL> from constants import MAX_ACTIONS_TO_TRY <EOL> from lfd . demonstration . demonstration import SceneState , GroundTruthRopeSceneState , AugmentedTrajectory , Demonstration <EOL> from lfd . environment . simulation import DynamicRopeSimulationRobotWorld <EOL> from lfd . environment . simulation_object import XmlSimulationObject , BoxSimulationObject , CylinderSimulationObject , RopeSimulationObject <EOL> from lfd . environment . environment import LfdEnvironment , GroundTruthRopeLfdEnvironment <EOL> from lfd . registration . registration import TpsRpmBijRegistrationFactory , TpsRpmRegistrationFactory , TpsSegmentRegistrationFactory , BatchGpuTpsRpmBijRegistrationFactory , BatchGpuTpsRpmRegistrationFactory <EOL> from lfd . transfer . transfer import PoseTrajectoryTransferer , FingerTrajectoryTransferer <EOL> from lfd . transfer . registration_transfer import TwoStepRegistrationAndTrajectoryTransferer , UnifiedRegistrationAndTrajectoryTransferer <EOL> from lfd . action_selection import GreedyActionSelection <EOL> from lfd . action_selection import FeatureActionSelection <EOL> from lfd . rapprentice import eval_util , util <EOL> from lfd . rapprentice import task_execution <EOL> from lfd . rapprentice . knot_classifier import isKnot as is_knot <EOL> from lfd . rapprentice . util import redprint , yellowprint <EOL> class GlobalVars : <EOL> exec_log = None <EOL> actions = None <EOL> actions_cache = None <EOL> demos = None <EOL> features = None <EOL> def eval_on_holdout ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) : <EOL> """<STR_LIT>""" <EOL> holdoutfile = h5py . File ( args . eval . holdoutfile , '<STR_LIT:r>' ) <EOL> holdout_items = eval_util . get_indexed_items ( holdoutfile , task_list = args . tasks , task_file = args . taskfile , i_start = args . i_start , i_end = args . i_end ) <EOL> rope_params = sim_util . RopeParams ( ) <EOL> if args . eval . rope_param_radius is not None : <EOL> rope_params . radius = args . eval . rope_param_radius <EOL> if args . eval . rope_param_angStiffness is not None : <EOL> rope_params . angStiffness = args . eval . rope_param_angStiffness <EOL> num_successes = <NUM_LIT:0> <EOL> num_total = <NUM_LIT:0> <EOL> for i_task , demo_id_rope_nodes in holdout_items : <EOL> redprint ( "<STR_LIT>" % i_task ) <EOL> init_rope_nodes = demo_id_rope_nodes [ "<STR_LIT>" ] [ : ] <EOL> rope = RopeSimulationObject ( "<STR_LIT>" , init_rope_nodes , rope_params ) <EOL> sim . add_objects ( [ rope ] ) <EOL> sim . settle ( step_viewer = args . animation ) <EOL> for i_step in range ( args . eval . num_steps ) : <EOL> redprint ( "<STR_LIT>" % ( i_task , i_step ) ) <EOL> sim_util . reset_arms_to_side ( sim ) <EOL> if args . animation : <EOL> sim . viewer . Step ( ) <EOL> sim_state = sim . get_state ( ) <EOL> sim . set_state ( sim_state ) <EOL> scene_state = lfd_env . observe_scene ( ) <EOL> handles = [ ] <EOL> if args . plotting : <EOL> handles . append ( sim . env . plot3 ( scene_state . cloud [ : , : <NUM_LIT:3> ] , <NUM_LIT:2> , scene_state . color if scene_state . color is not None else ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) ) ) <EOL> sim . viewer . Step ( ) <EOL> eval_stats = eval_util . EvalStats ( ) <EOL> start_time = time . time ( ) <EOL> if len ( scene_state . cloud ) == <NUM_LIT:0> : <EOL> redprint ( "<STR_LIT>" ) <EOL> break <EOL> try : <EOL> ( agenda , q_values_root ) , goal_found = action_selection . plan_agenda ( scene_state , i_step ) <EOL> except ValueError : <EOL> redprint ( "<STR_LIT>" ) <EOL> break <EOL> eval_stats . action_elapsed_time += time . time ( ) - start_time <EOL> eval_stats . generalized = True <EOL> num_actions_to_try = MAX_ACTIONS_TO_TRY if args . eval . search_until_feasible else <NUM_LIT:1> <EOL> for i_choice in range ( num_actions_to_try ) : <EOL> if q_values_root [ i_choice ] == - np . inf : <EOL> eval_stats . generalized = False <EOL> break <EOL> redprint ( "<STR_LIT>" % agenda [ i_choice ] ) <EOL> best_root_action = str ( agenda [ i_choice ] ) <EOL> start_time = time . time ( ) <EOL> try : <EOL> test_aug_traj = reg_and_traj_transferer . transfer ( GlobalVars . demos [ best_root_action ] , scene_state , plotting = args . plotting ) <EOL> except ValueError : <EOL> redprint ( "<STR_LIT>" ) <EOL> break <EOL> eval_stats . feasible , eval_stats . misgrasp = lfd_env . execute_augmented_trajectory ( test_aug_traj , step_viewer = args . animation , interactive = args . interactive , check_feasible = args . eval . check_feasible ) <EOL> eval_stats . exec_elapsed_time += time . time ( ) - start_time <EOL> if not args . eval . check_feasible or eval_stats . feasible : <EOL> break <EOL> else : <EOL> sim . set_state ( sim_state ) <EOL> knot = is_knot ( rope . rope . GetControlPoints ( ) ) <EOL> results = { '<STR_LIT>' : scene_state , '<STR_LIT>' : best_root_action , '<STR_LIT>' : q_values_root , '<STR_LIT>' : test_aug_traj , '<STR_LIT>' : eval_stats , '<STR_LIT>' : sim_state , '<STR_LIT>' : knot , '<STR_LIT>' : goal_found } <EOL> eval_util . save_task_results_step ( args . resultfile , i_task , i_step , results ) <EOL> if not eval_stats . generalized : <EOL> assert not knot <EOL> break <EOL> if args . eval . check_feasible and not eval_stats . feasible : <EOL> assert not knot <EOL> break <EOL> if knot : <EOL> num_successes += <NUM_LIT:1> <EOL> break ; <EOL> sim . remove_objects ( [ rope ] ) <EOL> num_total += <NUM_LIT:1> <EOL> redprint ( '<STR_LIT>' + str ( num_successes ) + '<STR_LIT:/>' + str ( num_total ) ) <EOL> redprint ( '<STR_LIT>' + str ( float ( num_successes ) / num_total ) ) <EOL> def eval_on_holdout_parallel ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) : <EOL> raise NotImplementedError <EOL> def replay_on_holdout ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) : <EOL> loadresultfile = h5py . File ( args . replay . loadresultfile , '<STR_LIT:r>' ) <EOL> loadresult_items = eval_util . get_indexed_items ( loadresultfile , task_list = args . tasks , task_file = args . taskfile , i_start = args . i_start , i_end = args . i_end ) <EOL> num_successes = <NUM_LIT:0> <EOL> num_total = <NUM_LIT:0> <EOL> for i_task , task_info in loadresult_items : <EOL> redprint ( "<STR_LIT>" % i_task ) <EOL> for i_step in range ( len ( task_info ) ) : <EOL> redprint ( "<STR_LIT>" % ( i_task , i_step ) ) <EOL> replay_results = eval_util . load_task_results_step ( args . replay . loadresultfile , i_task , i_step ) <EOL> sim_state = replay_results [ '<STR_LIT>' ] <EOL> if i_step > <NUM_LIT:0> : <EOL> sim_util . reset_arms_to_side ( sim ) <EOL> if sim . simulation_state_equal ( sim_state , sim . get_state ( ) ) : <EOL> yellowprint ( "<STR_LIT>" ) <EOL> else : <EOL> yellowprint ( "<STR_LIT>" ) <EOL> sim . set_state ( sim_state ) <EOL> if args . replay . simulate_traj_steps is not None and i_step not in args . replay . simulate_traj_steps : <EOL> continue <EOL> if i_step in args . replay . compute_traj_steps : <EOL> best_root_action = replay_results [ '<STR_LIT>' ] <EOL> scene_state = replay_results [ '<STR_LIT>' ] <EOL> handles = [ ] <EOL> if args . plotting : <EOL> handles . append ( sim . env . plot3 ( scene_state . cloud [ : , : <NUM_LIT:3> ] , <NUM_LIT:2> , scene_state . color if scene_state . color is not None else ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) ) ) <EOL> sim . viewer . Step ( ) <EOL> test_aug_traj = reg_and_traj_transferer . transfer ( GlobalVars . demos [ best_root_action ] , scene_state , plotting = args . plotting ) <EOL> else : <EOL> test_aug_traj = replay_results [ '<STR_LIT>' ] <EOL> feasible , misgrasp = lfd_env . execute_augmented_trajectory ( test_aug_traj , step_viewer = args . animation , interactive = args . interactive , check_feasible = args . eval . check_feasible ) <EOL> if replay_results [ '<STR_LIT>' ] : <EOL> num_successes += <NUM_LIT:1> <EOL> num_total += <NUM_LIT:1> <EOL> redprint ( '<STR_LIT>' + str ( num_successes ) + '<STR_LIT:/>' + str ( num_total ) ) <EOL> def parse_input_args ( ) : <EOL> parser = util . ArgumentParser ( ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> , help = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:1> , help = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , action = "<STR_LIT:store_true>" , help = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str , help = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , nargs = '<STR_LIT:*>' , metavar = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , default = - <NUM_LIT:1> , metavar = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , default = - <NUM_LIT:1> , metavar = "<STR_LIT>" ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str , default = '<STR_LIT>' ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str , default = '<STR_LIT>' ) <EOL> parser . add_argument ( "<STR_LIT>" , type = int , default = None ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str , default = "<STR_LIT>" ) <EOL> subparsers = parser . add_subparsers ( dest = '<STR_LIT>' ) <EOL> parser_eval = subparsers . add_parser ( '<STR_LIT>' ) <EOL> parser_eval . add_argument ( '<STR_LIT>' , type = str , nargs = '<STR_LIT:?>' , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( '<STR_LIT>' , type = str , nargs = '<STR_LIT:?>' , default = '<STR_LIT>' ) <EOL> parser . add_argument ( "<STR_LIT>" , type = str , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( '<STR_LIT>' , type = str , nargs = '<STR_LIT:?>' , choices = [ '<STR_LIT>' , '<STR_LIT>' ] ) <EOL> parser_eval . add_argument ( '<STR_LIT>' , type = str , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( '<STR_LIT>' , type = str , nargs = '<STR_LIT:?>' , choices = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , nargs = '<STR_LIT:?>' , choices = [ '<STR_LIT>' , '<STR_LIT>' ] , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , choices = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , nargs = '<STR_LIT:*>' , choices = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] , default = [ ] ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:1> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:1> , help = "<STR_LIT>" ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , default = '<STR_LIT>' ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , nargs = <NUM_LIT:6> , metavar = ( "<STR_LIT>" , "<STR_LIT>" , "<STR_LIT>" , "<STR_LIT>" , "<STR_LIT>" , "<STR_LIT>" ) , <EOL> default = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , help = "<STR_LIT>" ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , action = "<STR_LIT:store_true>" ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:1> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:1> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:5> , help = "<STR_LIT>" ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = float , default = <NUM_LIT:1.0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , default = None ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = str , default = None ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , type = int , default = <NUM_LIT:0> ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , action = "<STR_LIT:store_true>" ) <EOL> parser_eval . add_argument ( "<STR_LIT>" , action = "<STR_LIT:store_true>" , default = False ) <EOL> parser_replay = subparsers . add_parser ( '<STR_LIT>' ) <EOL> parser_replay . add_argument ( "<STR_LIT>" , type = str ) <EOL> parser_replay . add_argument ( "<STR_LIT>" , type = int , default = [ ] , nargs = '<STR_LIT:*>' , metavar = '<STR_LIT>' , help = "<STR_LIT>" ) <EOL> parser_replay . add_argument ( "<STR_LIT>" , type = int , default = None , nargs = '<STR_LIT:*>' , metavar = '<STR_LIT>' , <EOL> help = "<STR_LIT>" ) <EOL> args = parser . parse_args ( ) <EOL> if not args . animation : <EOL> args . plotting = <NUM_LIT:0> <EOL> return args <EOL> def setup_log_file ( args ) : <EOL> if args . log : <EOL> redprint ( "<STR_LIT>" % args . log ) <EOL> GlobalVars . exec_log = task_execution . ExecutionLog ( args . log ) <EOL> atexit . register ( GlobalVars . exec_log . close ) <EOL> GlobalVars . exec_log ( <NUM_LIT:0> , "<STR_LIT>" , args ) <EOL> def set_global_vars ( args ) : <EOL> if args . random_seed is not None : np . random . seed ( args . random_seed ) <EOL> GlobalVars . actions = h5py . File ( args . eval . actionfile , '<STR_LIT:r>' ) <EOL> actions_root , actions_ext = os . path . splitext ( args . eval . actionfile ) <EOL> GlobalVars . actions_cache = h5py . File ( actions_root + '<STR_LIT>' + actions_ext , '<STR_LIT:a>' ) <EOL> GlobalVars . demos = { } <EOL> for action , seg_info in GlobalVars . actions . iteritems ( ) : <EOL> if args . eval . ground_truth : <EOL> rope_nodes = seg_info [ '<STR_LIT>' ] [ ( ) ] <EOL> scene_state = GroundTruthRopeSceneState ( rope_nodes , settings . ROPE_RADIUS , upsample = args . eval . upsample , upsample_rad = args . eval . upsample_rad , downsample_size = args . eval . downsample_size ) <EOL> else : <EOL> full_cloud = seg_info [ '<STR_LIT>' ] [ ( ) ] <EOL> scene_state = SceneState ( full_cloud , downsample_size = args . eval . downsample_size ) <EOL> lr2arm_traj = { } <EOL> lr2finger_traj = { } <EOL> lr2ee_traj = { } <EOL> lr2open_finger_traj = { } <EOL> lr2close_finger_traj = { } <EOL> for lr in '<STR_LIT>' : <EOL> arm_name = { "<STR_LIT:l>" : "<STR_LIT>" , "<STR_LIT:r>" : "<STR_LIT>" } [ lr ] <EOL> lr2arm_traj [ lr ] = np . asarray ( seg_info [ arm_name ] ) <EOL> lr2finger_traj [ lr ] = sim_util . gripper_joint2gripper_l_finger_joint_values ( np . asarray ( seg_info [ '<STR_LIT>' % lr ] ) ) [ : , None ] <EOL> lr2ee_traj [ lr ] = np . asarray ( seg_info [ "<STR_LIT>" % lr ] [ '<STR_LIT>' ] ) <EOL> lr2open_finger_traj [ lr ] = np . zeros ( len ( lr2finger_traj [ lr ] ) , dtype = bool ) <EOL> lr2close_finger_traj [ lr ] = np . zeros ( len ( lr2finger_traj [ lr ] ) , dtype = bool ) <EOL> opening_inds , closing_inds = sim_util . get_opening_closing_inds ( lr2finger_traj [ lr ] ) <EOL> lr2open_finger_traj [ lr ] [ opening_inds ] = True <EOL> lr2close_finger_traj [ lr ] [ closing_inds ] = True <EOL> aug_traj = AugmentedTrajectory ( lr2arm_traj = lr2arm_traj , lr2finger_traj = lr2finger_traj , lr2ee_traj = lr2ee_traj , lr2open_finger_traj = lr2open_finger_traj , lr2close_finger_traj = lr2close_finger_traj ) <EOL> demo = Demonstration ( action , scene_state , aug_traj ) <EOL> GlobalVars . demos [ action ] = demo <EOL> def setup_lfd_environment_sim ( args ) : <EOL> actions = h5py . File ( args . eval . actionfile , '<STR_LIT:r>' ) <EOL> init_rope_xyz , init_joint_names , init_joint_values = sim_util . load_fake_data_segment ( actions , args . eval . fake_data_segment , args . eval . fake_data_transform ) <EOL> table_height = init_rope_xyz [ : , <NUM_LIT:2> ] . mean ( ) - <NUM_LIT> <EOL> sim_objs = [ ] <EOL> sim_objs . append ( XmlSimulationObject ( "<STR_LIT>" , dynamic = False ) ) <EOL> sim_objs . append ( BoxSimulationObject ( "<STR_LIT>" , [ <NUM_LIT:1> , <NUM_LIT:0> , table_height + ( - <NUM_LIT> + <NUM_LIT> ) ] , [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , dynamic = False ) ) <EOL> if '<STR_LIT>' in args . eval . obstacles : <EOL> sim_objs . append ( XmlSimulationObject ( "<STR_LIT>" , dynamic = False ) ) <EOL> if '<STR_LIT>' in args . eval . obstacles : <EOL> sim_objs . append ( BoxSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> ) ] , [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , dynamic = False ) ) <EOL> sim_objs . append ( BoxSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> * <NUM_LIT:2> + <NUM_LIT> ) ] , [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , dynamic = False ) ) <EOL> if '<STR_LIT>' in args . eval . obstacles : <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> ) ] , <NUM_LIT> , <NUM_LIT:1.> , dynamic = False ) ) <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , - <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> ) ] , <NUM_LIT> , <NUM_LIT:1.> , dynamic = False ) ) <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> ) ] , <NUM_LIT> , <NUM_LIT> , dynamic = False ) ) <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , [ <NUM_LIT> , - <NUM_LIT> , table_height + ( <NUM_LIT> + <NUM_LIT> ) ] , <NUM_LIT> , <NUM_LIT> , dynamic = False ) ) <EOL> sim = DynamicRopeSimulationRobotWorld ( ) <EOL> world = sim <EOL> sim . add_objects ( sim_objs ) <EOL> if args . eval . ground_truth : <EOL> lfd_env = GroundTruthRopeLfdEnvironment ( sim , world , upsample = args . eval . upsample , upsample_rad = args . eval . upsample_rad , downsample_size = args . eval . downsample_size ) <EOL> else : <EOL> lfd_env = LfdEnvironment ( sim , world , downsample_size = args . eval . downsample_size ) <EOL> dof_inds = sim_util . dof_inds_from_name ( sim . robot , '<STR_LIT:+>' . join ( init_joint_names ) ) <EOL> values , dof_inds = zip ( * [ ( value , dof_ind ) for value , dof_ind in zip ( init_joint_values , dof_inds ) if dof_ind != - <NUM_LIT:1> ] ) <EOL> sim . robot . SetDOFValues ( values , dof_inds ) <EOL> sim_util . reset_arms_to_side ( sim ) <EOL> if args . animation : <EOL> viewer = trajoptpy . GetViewer ( sim . env ) <EOL> if os . path . isfile ( args . window_prop_file ) and os . path . isfile ( args . camera_matrix_file ) : <EOL> print "<STR_LIT>" <EOL> window_prop = np . loadtxt ( args . window_prop_file ) <EOL> camera_matrix = np . loadtxt ( args . camera_matrix_file ) <EOL> try : <EOL> viewer . SetWindowProp ( * window_prop ) <EOL> viewer . SetCameraManipulatorMatrix ( camera_matrix ) <EOL> except : <EOL> print "<STR_LIT>" <EOL> else : <EOL> print "<STR_LIT>" <EOL> print "<STR_LIT>" <EOL> viewer . Idle ( ) <EOL> print "<STR_LIT>" <EOL> try : <EOL> window_prop = viewer . GetWindowProp ( ) <EOL> camera_matrix = viewer . GetCameraManipulatorMatrix ( ) <EOL> np . savetxt ( args . window_prop_file , window_prop , fmt = '<STR_LIT>' ) <EOL> np . savetxt ( args . camera_matrix_file , camera_matrix ) <EOL> except : <EOL> print "<STR_LIT>" <EOL> viewer . Step ( ) <EOL> if args . eval . dof_limits_factor != <NUM_LIT:1.0> : <EOL> assert <NUM_LIT:0> < args . eval . dof_limits_factor and args . eval . dof_limits_factor <= <NUM_LIT:1.0> <EOL> active_dof_indices = sim . robot . GetActiveDOFIndices ( ) <EOL> active_dof_limits = sim . robot . GetActiveDOFLimits ( ) <EOL> for lr in '<STR_LIT>' : <EOL> manip_name = { "<STR_LIT:l>" : "<STR_LIT>" , "<STR_LIT:r>" : "<STR_LIT>" } [ lr ] <EOL> dof_inds = sim . robot . GetManipulator ( manip_name ) . GetArmIndices ( ) <EOL> limits = np . asarray ( sim . robot . GetDOFLimits ( dof_inds ) ) <EOL> limits_mean = limits . mean ( axis = <NUM_LIT:0> ) <EOL> limits_width = np . diff ( limits , axis = <NUM_LIT:0> ) <EOL> new_limits = limits_mean + args . eval . dof_limits_factor * np . r_ [ - limits_width / <NUM_LIT> , limits_width / <NUM_LIT> ] <EOL> for i , ind in enumerate ( dof_inds ) : <EOL> active_dof_limits [ <NUM_LIT:0> ] [ active_dof_indices . tolist ( ) . index ( ind ) ] = new_limits [ <NUM_LIT:0> , i ] <EOL> active_dof_limits [ <NUM_LIT:1> ] [ active_dof_indices . tolist ( ) . index ( ind ) ] = new_limits [ <NUM_LIT:1> , i ] <EOL> sim . robot . SetDOFLimits ( active_dof_limits [ <NUM_LIT:0> ] , active_dof_limits [ <NUM_LIT:1> ] ) <EOL> return lfd_env , sim <EOL> def setup_registration_and_trajectory_transferer ( args , sim ) : <EOL> if args . eval . batch : <EOL> if args . eval . reg_type == '<STR_LIT>' : <EOL> reg_factory = BatchGpuTpsRpmRegistrationFactory ( GlobalVars . demos , args . eval . actionfile ) <EOL> elif args . eval . reg_type == '<STR_LIT>' : <EOL> reg_factory = BatchGpuTpsRpmBijRegistrationFactory ( GlobalVars . demos , args . eval . actionfile ) <EOL> else : <EOL> raise RuntimeError ( "<STR_LIT>" % args . eval . reg_type ) <EOL> else : <EOL> if args . eval . reg_type == '<STR_LIT>' : <EOL> reg_factory = TpsSegmentRegistrationFactory ( GlobalVars . demos ) <EOL> elif args . eval . reg_type == '<STR_LIT>' : <EOL> reg_factory = TpsRpmRegistrationFactory ( GlobalVars . demos ) <EOL> elif args . eval . reg_type == '<STR_LIT>' : <EOL> reg_factory = TpsRpmBijRegistrationFactory ( GlobalVars . demos , actionfile = args . eval . actionfile ) <EOL> else : <EOL> raise RuntimeError ( "<STR_LIT>" % args . eval . reg_type ) <EOL> if args . eval . transferopt == '<STR_LIT>' or args . eval . transferopt == '<STR_LIT>' : <EOL> traj_transferer = PoseTrajectoryTransferer ( sim , args . eval . beta_pos , args . eval . beta_rot , args . eval . gamma , args . eval . use_collision_cost ) <EOL> if args . eval . transferopt == '<STR_LIT>' : <EOL> traj_transferer = FingerTrajectoryTransferer ( sim , args . eval . beta_pos , args . eval . gamma , args . eval . use_collision_cost , init_trajectory_transferer = traj_transferer ) <EOL> else : <EOL> raise RuntimeError ( "<STR_LIT>" % args . eval . transferopt ) <EOL> if args . eval . unified : <EOL> reg_and_traj_transferer = UnifiedRegistrationAndTrajectoryTransferer ( reg_factory , traj_transferer ) <EOL> else : <EOL> reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer ( reg_factory , traj_transferer ) <EOL> return reg_and_traj_transferer <EOL> def get_features ( args ) : <EOL> feat_type = args . eval . feature_type <EOL> if feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import BatchRCFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import MulFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import QuadSimpleMulFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import QuadSimpleMulIndFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import QuadSimpleMulMapIndFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import QuadSimpleMulBendIndFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import SimpleMulFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import SimpleMulGripperFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import SimpleMulMapIndFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import LandmarkFeats as feat <EOL> elif feat_type == '<STR_LIT>' : <EOL> from lfd . mmqe . features import TimestepActionMulFeats as feat <EOL> else : <EOL> raise ValueError ( '<STR_LIT>' ) <EOL> feats = feat ( args . eval . actionfile ) <EOL> try : <EOL> feats . set_landmark_file ( args . landmarkfile ) <EOL> except AttributeError : <EOL> pass <EOL> if args . eval . weightfile : <EOL> feats . load_weights ( args . eval . weightfile ) <EOL> GlobalVars . features = feats <EOL> return feats <EOL> def main ( ) : <EOL> args = parse_input_args ( ) <EOL> if args . subparser_name == "<STR_LIT>" : <EOL> eval_util . save_results_args ( args . resultfile , args ) <EOL> elif args . subparser_name == "<STR_LIT>" : <EOL> loaded_args = eval_util . load_results_args ( args . replay . loadresultfile ) <EOL> assert '<STR_LIT>' not in vars ( args ) <EOL> args . eval = loaded_args . eval <EOL> else : <EOL> raise RuntimeError ( "<STR_LIT>" ) <EOL> setup_log_file ( args ) <EOL> set_global_vars ( args ) <EOL> trajoptpy . SetInteractive ( args . interactive ) <EOL> lfd_env , sim = setup_lfd_environment_sim ( args ) <EOL> reg_and_traj_transferer = setup_registration_and_trajectory_transferer ( args , sim ) <EOL> if args . eval . action_selection == '<STR_LIT>' : <EOL> get_features ( args ) <EOL> if args . eval . action_selection == '<STR_LIT>' : <EOL> action_selection = GreedyActionSelection ( reg_and_traj_transferer . registration_factory ) <EOL> else : <EOL> action_selection = FeatureActionSelection ( reg_and_traj_transferer . registration_factory , GlobalVars . features , GlobalVars . actions , GlobalVars . demos , simulator = reg_and_traj_transferer , lfd_env = lfd_env , width = args . eval . width , depth = args . eval . depth ) <EOL> if args . subparser_name == "<STR_LIT>" : <EOL> start = time . time ( ) <EOL> if args . eval . parallel : <EOL> eval_on_holdout_parallel ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) <EOL> else : <EOL> eval_on_holdout ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) <EOL> print "<STR_LIT>" . format ( time . time ( ) - start ) <EOL> elif args . subparser_name == "<STR_LIT>" : <EOL> replay_on_holdout ( args , action_selection , reg_and_traj_transferer , lfd_env , sim ) <EOL> else : <EOL> raise RuntimeError ( "<STR_LIT>" ) <EOL> if __name__ == "<STR_LIT:__main__>" : <EOL> main ( ) </s>
