def replace_rope(sim, new_rope, animation): rope_sim_obj = None for sim_obj in sim.sim_objs: if isinstance(sim_obj, RopeSimulationObject): rope_sim_obj = sim_obj break if rope_sim_obj: sim.remove_objects([rope_sim_obj]) rope = RopeSimulationObject("rope", new_rope, sim_util.RopeParams()) sim.add_objects([rope]) sim.settle(step_viewer=animation)
def label_demos_parallel(args, transferer, lfd_env, sim): outfile = h5py.File(args.eval.outfile, 'a') rope_params = sim_util.RopeParams() rope_params.radius = settings.ROPE_RADIUS_THICK if args.eval.rope_param_radius is not None: rope_params.radius = args.eval.rope_param_radius if args.eval.rope_param_angStiffness is not None: rope_params.angStiffness = args.eval.rope_param_angStiffness # TODO pass in rope params to sample_rope_state (init_rope_nodes, demo_id) = sample_rope_state(args.eval, sim, args.animation) resample = False labeled_data = [] while True: if args.animation: sim.viewer.Step() sim_state = sim.get_state() sim.set_state(sim_state) scene_state = lfd_env.observe_scene() # plot cloud of the test scene handles = [] if args.plotting: handles.append( sim.env.plot3( scene_state.cloud[:, :3], 2, scene_state.color if scene_state.color is not None else (0, 0, 1))) sim.viewer.Step() costs = transferer.registration_factory.batch_cost(scene_state) best_keys = sorted(costs, key=costs.get) for seg_name in best_keys: traj = transferer.transfer(GlobalVars.demos[seg_name], scene_state, plotting=args.plotting) feasible, misgrasp = lfd_env.execute_augmented_trajectory( traj, step_viewer=args.animation, interactive=args.interactive) sim_util.reset_arms_to_side(sim) sim.settle(step_viewer=args.animation) if not feasible or misgrasp: print 'Feasible: ', feasible print 'Misgrasp: ', misgrasp if misgrasp: sim.set_state(sim_state) continue print "y accepts this action" print "n rejects this action" print "r resamples rope state" print "f to save this as a failure" print "C-c safely quits" user_input = lower(raw_input("What to do?")) success = False if user_input == 'y': success = True labeled_data.append((scene_state, seg_name)) if isFig8Knot(get_rope_nodes(sim)): save_success(outfile, labeled_data) labeled_data = [] sim.set_state(sample_rope_state(args.eval, sim)) break elif user_input == 'n': sim.set_state(sim_state) continue elif user_input == 'r': break elif user_input == 'f': sim.set_state(sim_state) save_failure(outfile, lfd_env.observe_scene()) continue if not success: labeled_data = [] sim.set_state(sample_rope_state(args.eval, sim))
def eval_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim): """TODO Args: action_selection: ActionSelection reg_and_traj_transferer: RegistrationAndTrajectoryTransferer lfd_env: LfdEnvironment sim: DynamicSimulation """ holdoutfile = h5py.File(args.eval.holdoutfile, 'r') 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) rope_params = sim_util.RopeParams() if args.eval.rope_param_radius is not None: rope_params.radius = args.eval.rope_param_radius if args.eval.rope_param_angStiffness is not None: rope_params.angStiffness = args.eval.rope_param_angStiffness num_successes = 0 num_total = 0 for i_task, demo_id_rope_nodes in holdout_items: redprint("task %s" % i_task) init_rope_nodes = demo_id_rope_nodes["rope_nodes"][:] rope = RopeSimulationObject("rope", init_rope_nodes, rope_params) sim.add_objects([rope]) sim.settle(step_viewer=args.animation) for i_step in range(args.eval.num_steps): redprint("task %s step %i" % (i_task, i_step)) sim_util.reset_arms_to_side(sim) if args.animation: sim.viewer.Step() sim_state = sim.get_state() sim.set_state(sim_state) scene_state = lfd_env.observe_scene() # plot cloud of the test scene handles = [] if args.plotting: handles.append(sim.env.plot3(scene_state.cloud[:,:3], 2, scene_state.color if scene_state.color is not None else (0,0,1))) sim.viewer.Step() eval_stats = eval_util.EvalStats() start_time = time.time() if len(scene_state.cloud) == 0: redprint("Detected 0 points in scene") break try: (agenda, q_values_root), goal_found = action_selection.plan_agenda(scene_state, i_step) except ValueError: #e.g. if cloud is empty - any action is hopeless redprint("**Raised Value Error during action selection") break eval_stats.action_elapsed_time += time.time() - start_time eval_stats.generalized = True num_actions_to_try = MAX_ACTIONS_TO_TRY if args.eval.search_until_feasible else 1 for i_choice in range(num_actions_to_try): if q_values_root[i_choice] == -np.inf: # none of the demonstrations generalize eval_stats.generalized = False break redprint("TRYING %s"%agenda[i_choice]) best_root_action = str(agenda[i_choice]) start_time = time.time() try: test_aug_traj = reg_and_traj_transferer.transfer(GlobalVars.demos[best_root_action], scene_state, plotting=args.plotting) except ValueError: # If something is cloud/traj is empty or something redprint("**Raised value error during traj transfer") break 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) eval_stats.exec_elapsed_time += time.time() - start_time if not args.eval.check_feasible or eval_stats.feasible: # try next action if TrajOpt cannot find feasible action and we care about feasibility break else: sim.set_state(sim_state) knot = is_knot(rope.rope.GetControlPoints()) results = {'scene_state':scene_state, 'best_action':best_root_action, 'values':q_values_root, 'aug_traj':test_aug_traj, 'eval_stats':eval_stats, 'sim_state':sim_state, 'knot':knot, 'goal_found': goal_found} eval_util.save_task_results_step(args.resultfile, i_task, i_step, results) if not eval_stats.generalized: assert not knot break if args.eval.check_feasible and not eval_stats.feasible: # Skip to next knot tie if the action is infeasible -- since # that means all future steps (up to 5) will have infeasible trajectories assert not knot break if knot: num_successes += 1 break; sim.remove_objects([rope]) num_total += 1 redprint('Eval Successes / Total: ' + str(num_successes) + '/' + str(num_total)) redprint('Success Rate: ' + str(float(num_successes)/num_total))
helix_ang0 = 0 helix_ang1 = 4 * np.pi helix_radius = .2 helix_center = np.r_[.6, 0] helix_height0 = table_height + .15 helix_height1 = table_height + .15 + .3 helix_length = np.linalg.norm(np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0]) num = np.round(helix_length / .02) helix_angs = np.linspace(helix_ang0, helix_ang1, num) helix_heights = np.linspace(helix_height0, helix_height1, num) init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights] rope_params = sim_util.RopeParams() cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1], dynamic=False)) sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append( CylinderSimulationObject("cyl0",
def setUp(self): table_height = 0.77 helix_ang0 = 0 helix_ang1 = 4 * np.pi helix_radius = .2 helix_center = np.r_[.6, 0] helix_height0 = table_height + .15 helix_height1 = table_height + .15 + .3 helix_length = np.linalg.norm( np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0]) num = np.round(helix_length / .02) helix_angs = np.linspace(helix_ang0, helix_ang1, num) helix_heights = np.linspace(helix_height0, helix_height1, num) init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights] rope_params = sim_util.RopeParams() cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1], dynamic=False)) sim_objs.append( RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append( CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True)) sim_objs.append( CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True)) self.sim = DynamicSimulation() self.sim.add_objects(sim_objs) self.sim.robot.SetDOFValues( [0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(self.sim) # rotate cylinders by 90 deg for i in range(2): bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i) T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0])) T[:3, 3] = bt_cyl.GetTransform()[:3, 3] bt_cyl.SetTransform( T ) # SetTransform needs to be used in the Bullet object, not the openrave body self.sim.update()