def load_simulation(args, sim_env): sim_env.env = openravepy.Environment() sim_env.env.StopSimulation() sim_env.env.Load("robots/pr2-beta-static.zae") sim_env.robot = sim_env.env.GetRobots()[0] actions = h5py.File(args.actionfile, 'r') fakedatafile = h5py.File(args.fakedatafile, 'r') GlobalVars.init_tfm = fakedatafile['init_tfm'][()] init_rope_xyz, _ = sim_util.load_fake_data_segment(sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform) # this also sets the torso (torso_lift_joint) to the height in the data # Set table height to correct height of first rope in holdout set holdoutfile = h5py.File(args.holdoutfile, 'r') first_holdout = holdoutfile[holdoutfile.keys()[0]] init_rope_xyz = first_holdout['rope_nodes'][:] if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r': init_rope_xyz = init_rope_xyz.dot(GlobalVars.init_tfm[:3,:3].T) + GlobalVars.init_tfm[:3,3][None,:] table_height = init_rope_xyz[:,2].mean() - .02 # Before: .02 table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) sim_env.env.LoadData(table_xml) if 'bookshelve' in args.obstacles: sim_env.env.Load("data/bookshelves.env.xml") if 'boxes' in args.obstacles: sim_env.env.LoadData(sim_util.make_box_xml("box0", [.7,.43,table_height+(.01+.12)], [.12,.12,.12])) sim_env.env.LoadData(sim_util.make_box_xml("box1", [.74,.47,table_height+(.01+.12*2+.08)], [.08,.08,.08])) cc = trajoptpy.GetCollisionChecker(sim_env.env) for gripper_link in [link for link in sim_env.robot.GetLinks() if 'gripper' in link.GetName()]: cc.ExcludeCollisionPair(gripper_link, sim_env.env.GetKinBody('table').GetLinks()[0]) sim_util.reset_arms_to_side(sim_env) if args.animation: sim_env.viewer = trajoptpy.GetViewer(sim_env.env) if args.animation > 1 and os.path.isfile(args.window_prop_file) and os.path.isfile(args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: sim_env.viewer.SetWindowProp(*window_prop) sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" sim_env.viewer.Idle() print "saving window and camera properties" try: window_prop = sim_env.viewer.GetWindowProp() camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
def load_simulation(args, sim_env): sim_env.env = openravepy.Environment() sim_env.env.StopSimulation() actions = h5py.File(args.actionfile, 'r') fakedatafile = h5py.File(args.fakedatafile, 'r') GlobalVars.init_tfm = fakedatafile['init_tfm'][()] init_rope_xyz, _ = sim_util.load_fake_data_segment(sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform, floating=True) # this also sets the torso (torso_lift_joint) to the height in the data GlobalVars.init_tfm = GlobalVars.init_tfm # Set table height to correct height of first rope in holdout set holdoutfile = h5py.File(args.holdoutfile, 'r') first_holdout = holdoutfile[holdoutfile.keys()[0]] init_rope_xyz = first_holdout['rope_nodes'][:] if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r': init_rope_xyz = init_rope_xyz.dot(GlobalVars.init_tfm[:3,:3].T) + GlobalVars.init_tfm[:3,3][None,:] table_height = init_rope_xyz[:,2].mean() - .02 # Before: .02 GlobalVars.table_height = table_height table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) sim_env.env.LoadData(table_xml) sim_util.reset_arms_to_side(sim_env, floating=True) if args.animation: sim_env.viewer = trajoptpy.GetViewer(sim_env.env) table = sim_env.env.GetKinBody('table') sim_env.viewer.SetTransparency(table, 0.4) if args.animation > 1 and os.path.isfile(args.window_prop_file) and os.path.isfile(args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: sim_env.viewer.SetWindowProp(*window_prop) sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" sim_env.viewer.Idle() print "saving window and camera properties" try: window_prop = sim_env.viewer.GetWindowProp() camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
def load_simulation(args, sim_env): sim_env.env = openravepy.Environment() sim_env.env.StopSimulation() sim_env.env.Load("robots/pr2-beta-static.zae") sim_env.robot = sim_env.env.GetRobots()[0] actions = h5py.File(args.actionfile, 'r') fakedatafile = h5py.File(args.fakedatafile, 'r') GlobalVars.init_tfm = fakedatafile['init_tfm'][()] init_rope_xyz, _ = sim_util.load_fake_data_segment( sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform ) # this also sets the torso (torso_lift_joint) to the height in the data # Set table height to correct height of first rope in holdout set holdoutfile = h5py.File(args.holdoutfile, 'r') first_holdout = holdoutfile[holdoutfile.keys()[0]] init_rope_xyz = first_holdout['rope_nodes'][:] if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r': init_rope_xyz = init_rope_xyz.dot( GlobalVars.init_tfm[:3, :3].T) + GlobalVars.init_tfm[:3, 3][None, :] table_height = init_rope_xyz[:, 2].mean() - .02 # Before: .02 table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) sim_env.env.LoadData(table_xml) if 'bookshelve' in args.obstacles: sim_env.env.Load("data/bookshelves.env.xml") if 'boxes' in args.obstacles: sim_env.env.LoadData( sim_util.make_box_xml("box0", [.7, .43, table_height + (.01 + .12)], [.12, .12, .12])) sim_env.env.LoadData( sim_util.make_box_xml( "box1", [.74, .47, table_height + (.01 + .12 * 2 + .08)], [.08, .08, .08])) cc = trajoptpy.GetCollisionChecker(sim_env.env) for gripper_link in [ link for link in sim_env.robot.GetLinks() if 'gripper' in link.GetName() ]: cc.ExcludeCollisionPair(gripper_link, sim_env.env.GetKinBody('table').GetLinks()[0]) sim_util.reset_arms_to_side(sim_env) if args.animation: sim_env.viewer = trajoptpy.GetViewer(sim_env.env) if args.animation > 1 and os.path.isfile( args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: sim_env.viewer.SetWindowProp(*window_prop) sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" sim_env.viewer.Idle() print "saving window and camera properties" try: window_prop = sim_env.viewer.GetWindowProp() camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."