def _test_plan(test_obj, plan): print "testing plan: {}".format(plan.actions) callback = None viewer = None """ Uncomment out lines below to see optimization. """ viewer = OpenRAVEViewer.create_viewer() # def callback(): # solver._update_ll_params() # # obj_list = viewer._get_plan_obj_list(plan) # # # viewer.draw_traj(obj_list, [0,9,19,20,29,38]) # # # viewer.draw_traj(obj_list, range(19,30)) # # # viewer.draw_traj(obj_list, range(29,39)) # # # viewer.draw_traj(obj_list, [38]) # # # viewer.draw_traj(obj_list, range(19,39)) # # # viewer.draw_plan_range(plan, [0,19,38]) # viewer.draw_plan(plan) # time.sleep(0.03) """ """ solver = can_solver.CanSolver() solver.solve(plan, callback=callback, n_resamples=0, verbose=True) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() # if viewer != None: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t) test_obj.assertTrue(plan.satisfied())
def _test_backtrack_plan(test_obj, plan, n_resamples=0): print "testing plan: {}".format(plan.actions) callback = None viewer = None solver = can_solver.CanSolver() """ Uncomment out lines below to see optimization. """ viewer = OpenRAVEViewer.create_viewer() animate = get_animate_fn(viewer, plan) def callback(a): solver._update_ll_params() # viewer.clear() viewer.draw_plan_range(plan, a.active_timesteps) # time.sleep(0.3) """ """ solver.backtrack_solve(plan, callback=None, anum=0, verbose=True) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() # if viewer != None: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t) # import ipdb; ipdb.set_trace() test_obj.assertTrue(plan.satisfied(0))
def test_resample_ee_reachable(self): domain, problem, params, plan = load_environment( "../domains/baxter_domain/baxter.domain", "../domains/baxter_domain/baxter_probs/grasp_1234_1.prob") env = problem.env objLst = [i[1] for i in params.items() if not i[1].is_symbol()] view = OpenRAVEViewer.create_viewer(env) view.draw(objLst, 0, 0.7) baxter = params['baxter'] robot_pose = params['robot_init_pose'] ee_target = params['ee_target0'] can = params['can0'] # dof = robot.GetActiveDOFValues() pred = baxter_predicates.BaxterEEReachablePos( "resample_tester", [baxter, robot_pose, ee_target], ["Robot", "RobotPose", "EEPose"], env) # Initialize the trajectory of each parameters ee_target.value, ee_target.rotation = can.pose, can.rotation baxter.rArmPose = np.zeros((7, 7)) baxter.lArmPose = np.zeros((7, 7)) baxter.rGripper = 0.02 * np.ones((1, 7)) baxter.lGripper = 0.02 * np.ones((1, 7)) baxter.pose = np.zeros((1, 7)) baxter._free_attrs['rArmPose'] = np.ones((7, 7)) # Having initial Arm Pose. not supposed to be Ture self.assertFalse(pred.test(3)) val, attr_inds = baxter_sampling.resample_eereachable( pred, False, 3, plan) self.assertTrue(pred.test(3))
def _test_plan(plan, method='SQP', plot=False, animate=True, verbose=False, early_converge=False): print "testing plan: {}".format(plan.actions) if not plot: callback = None viewer = None else: viewer = OpenRAVEViewer.create_viewer() if method == 'SQP': def callback(): namo_solver._update_ll_params() # viewer.draw_plan_range(plan, range(57, 77)) # displays putdown action # viewer.draw_plan_range(plan, range(38, 77)) # displays moveholding and putdown action viewer.draw_plan(plan) # viewer.draw_cols(plan) time.sleep(0.3) elif method == 'Backtrack': def callback(a): namo_solver._update_ll_params() viewer.clear() viewer.draw_plan_range(plan, a.active_timesteps) time.sleep(0.3) namo_solver = ll_solver.NAMOSolver(early_converge=early_converge) start = time.time() if method == 'SQP': success = namo_solver.solve(plan, callback=callback, verbose=verbose) elif method == 'Backtrack': success = namo_solver.backtrack_solve(plan, callback=callback, verbose=verbose) print "Solve Took: {}\tSolution Found: {}".format(time.time() - start, success) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() if animate: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t)
def test_rrt_planner(self): # Adopting examples from openrave domain, problem, params = load_environment( '../domains/baxter_domain/baxter.domain', '../domains/baxter_domain/baxter_probs/grasp_1234_1.prob') env = Environment() # create openrave environment objLst = [i[1] for i in params.items() if not i[1].is_symbol()] view = OpenRAVEViewer.create_viewer(env) view.draw(objLst, 0, 0.7) can_body = view.name_to_rave_body["can0"] baxter_body = view.name_to_rave_body["baxter"] can = can_body.env_body robot = baxter_body.env_body dof = robot.GetActiveDOFValues() inds = baxter_body._geom.dof_map['rArmPose'] r_init = params['robot_init_pose'] r_end = params['robot_end_pose'] dof[inds] = r_init.rArmPose.flatten() robot.SetActiveDOFValues(dof) robot.SetActiveDOFs(inds) # set joints the first 4 dofs plan_params = Planner.PlannerParameters() plan_params.SetRobotActiveJoints(robot) plan_params.SetGoalConfig( [0.7, -0.204, 0.862, 1.217, 2.731, 0.665, 2.598]) # set goal to all ones # forces parabolic planning with 40 iterations traj = RaveCreateTrajectory(env, '') # Using openrave built in planner trajectory = {} plan_params.SetExtraParameters( """ <_postprocessing planner="parabolicsmoother"> <_nmaxiterations>17</_nmaxiterations> </_postprocessing>""") trajectory["BiRRT"] = planing(env, robot, plan_params, traj, 'BiRRT') # 3.5s # trajectory["BasicRRT"] = planing(env, robot, plan_params, traj, 'BasicRRT') # 0.05s can't run it by its own # trajectory["ExplorationRRT"] = planing(env, robot, plan_params, traj, 'ExplorationRRT') # 0.03s # plan_params.SetExtraParameters('<range>0.2</range>') # Using OMPL planner # trajectory["OMPL_RRTConnect"] = planing(env, robot, plan_params, traj, 'OMPL_RRTConnect') # 1.5s # trajectory["OMPL_RRT"] = planing(env, robot, plan_params, traj, 'OMPL_RRT') # 10s # trajectory["OMPL_RRTstar"] = planing(env, robot, plan_params, traj, 'OMPL_RRTstar') # 10s # trajectory["OMPL_TRRT"] = planing(env, robot, plan_params, traj, 'OMPL_TRRT') # 10s # trajectory["OMPL_pRRT"] = planing(env, robot, plan_params, traj, 'OMPL_pRRT') # Having issue, freeze # trajectory["OMPL_LazyRRT"] = planing(env, robot, plan_params, traj, 'OMPL_LazyRRT') # 1.5s - 10s unsatble # ompl_traj = trajectory["OMPL_RRTConnect"] or_traj = trajectory["BiRRT"] result = process_traj(or_traj, 20) self.assertTrue(result.shape[1] == 20)
def test_can_world(self): domain, problem, params = load_environment( '../domains/baxter_domain/baxter.domain', '../domains/baxter_domain/baxter_training_probs/grasp_training_4321_0.prob' ) geom = params['can0'].geom params['can0'].geom = GreenCan(geom.radius, geom.height) env = Environment() objLst = [i[1] for i in params.items() if not i[1].is_symbol()] view = OpenRAVEViewer(env) view.draw(objLst, 0, 0.7) can_body = view.name_to_rave_body["can0"] baxter_body = view.name_to_rave_body["baxter"] can = can_body.env_body robot = baxter_body.env_body dof = robot.GetActiveDOFValues() targ = params['target1'] # import ipdb; ipdb.set_trace() can_body.set_pose(targ.value, targ.rotation)
def _test_plan(test_obj, plan): from core.util_classes.viewer import OpenRAVEViewer from pma import ll_solver print "testing plan: {}".format(plan.actions) callback = None viewer = None """ Uncomment out lines below to see optimization. """ viewer = OpenRAVEViewer.create_viewer() def callback(): can_solver._update_ll_params() # obj_list = viewer._get_plan_obj_list(plan) # # viewer.draw_traj(obj_list, [0,9,19,20,29,38]) # # viewer.draw_traj(obj_list, range(19,30)) # # viewer.draw_traj(obj_list, range(29,39)) # # viewer.draw_traj(obj_list, [38]) # # viewer.draw_traj(obj_list, range(19,39)) # # viewer.draw_plan_range(plan, [0,19,38]) viewer.draw_plan(plan) time.sleep(0.03) """ """ can_solver = CanSolver() can_solver.solve(plan, callback=callback, n_resamples=0) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() # if viewer != None: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t) import ipdb ipdb.set_trace() test_obj.assertTrue(plan.satisfied(1e-4))
def _test_plan(test_obj, plan): from core.util_classes.viewer import OpenRAVEViewer from pma import ll_solver print "testing plan: {}".format(plan.actions) callback = None viewer = None """ Uncomment out lines below to see optimization. """ viewer = OpenRAVEViewer.create_viewer() def callback(): can_solver._update_ll_params() # obj_list = viewer._get_plan_obj_list(plan) # # viewer.draw_traj(obj_list, [0,9,19,20,29,38]) # # viewer.draw_traj(obj_list, range(19,30)) # # viewer.draw_traj(obj_list, range(29,39)) # # viewer.draw_traj(obj_list, [38]) # # viewer.draw_traj(obj_list, range(19,39)) # # viewer.draw_plan_range(plan, [0,19,38]) viewer.draw_plan(plan) time.sleep(0.03) """ """ can_solver = CanSolver() can_solver.solve(plan, callback=callback, n_resamples=0) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() # if viewer != None: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t) import ipdb; ipdb.set_trace() test_obj.assertTrue(plan.satisfied(1e-4))
def _test_plan(plan, method='SQP', plot=False, animate=True, verbose=False, early_converge=False): print "testing plan: {}".format(plan.actions) if not plot: callback = None viewer = None else: viewer = OpenRAVEViewer.create_viewer() if method=='SQP': def callback(): namo_solver._update_ll_params() # viewer.draw_plan_range(plan, range(57, 77)) # displays putdown action # viewer.draw_plan_range(plan, range(38, 77)) # displays moveholding and putdown action viewer.draw_plan(plan) # viewer.draw_cols(plan) time.sleep(0.3) elif method == 'Backtrack': def callback(a): namo_solver._update_ll_params() viewer.clear() viewer.draw_plan_range(plan, a.active_timesteps) time.sleep(0.3) namo_solver = ll_solver.NAMOSolver(early_converge=early_converge) start = time.time() if method == 'SQP': success = namo_solver.solve(plan, callback=callback, verbose=verbose) elif method == 'Backtrack': success = namo_solver.backtrack_solve(plan, callback=callback, verbose=verbose) print "Solve Took: {}\tSolution Found: {}".format(time.time() - start, success) fp = plan.get_failed_preds() _, _, t = plan.get_failed_pred() if animate: viewer = OpenRAVEViewer.create_viewer() viewer.animate_plan(plan) if t < plan.horizon: viewer.draw_plan_ts(plan, t)
def setUp(self): domain_fname = '../domains/can_domain/can.domain' d_c = main.parse_file_to_dict(domain_fname) domain = parse_domain_config.ParseDomainConfig.parse(d_c) hls = hl_solver.FFSolver(d_c) def get_plan(p_fname, plan_str=None): p_c = main.parse_file_to_dict(p_fname) problem = parse_problem_config.ParseProblemConfig.parse( p_c, domain) abs_problem = hls.translate_problem(problem) if plan_str is not None: return hls.get_plan(plan_str, domain, problem) # view = OpenRAVEViewer() # robot = problem.init_state.params['pr2'] # table = problem.init_state.params['table'] # cans = [] # for param_name, param in problem.init_state.params.iteritems(): # if "can" in param_name: # cans.append(param) # objs = [robot, table] # objs.extend(cans) # view.draw(objs, 0, 0.7) return hls.solve(abs_problem, domain, problem) self.bmove = get_plan( '../domains/can_domain/can_probs/can_1234_0.prob') # self.move_obs = get_plan('../domains/can_domain/can_probs/move_obs.prob') # self.move_no_obs = get_plan('../domains/can_domain/can_probs/move.prob') # self.move_no_obs = get_plan('../domains/can_domain/can_probs/can_1234_0.prob') # self.grasp = get_plan('../domains/can_domain/can_probs/grasp.prob') # self.grasp = get_plan('../domains/can_domain/can_probs/grasp_rot.prob') # self.grasp_gen = get_plan('../domains/can_domain/can_probs/can_grasp_1234_0.prob') # self.moveholding = get_plan('../domains/can_domain/can_probs/can_1234_0.prob', ['0: MOVETOHOLDING PR2 ROBOT_INIT_POSE ROBOT_END_POSE CAN0']) # # self.moveholding = get_plan('../domains/can_domain/can_probs/can_1234_0.prob') # self.gen_plan = get_plan('../domains/can_domain/can_probs/can_1234_0.prob') # self.grasp_obstructs1 = get_plan('../domains/can_domain/can_probs/can_grasp_1234_1.prob', ['0: GRASP PR2 CAN0 TARGET0 PDP_TARGET0 EE_TARGET0 PDP_TARGET0']) # self.grasp_obstructs0 = get_plan('../domains/can_domain/can_probs/can_grasp_1234_0.prob', ['0: GRASP PR2 CAN0 TARGET0 PDP_TARGET0 EE_TARGET0 PDP_TARGET0']) # self.grasp_obstructs = get_plan('../domains/can_domain/can_probs/can_grasp_1234_4.prob', ['0: GRASP PR2 CAN0 TARGET0 PDP_TARGET0 EE_TARGET0 PDP_TARGET0']) if VIEWER: self.viewer = OpenRAVEViewer.create_viewer() else: self.viewer = None
def resample_bp_around_target(pred, t, plan, target_pose, dist=OBJ_RING_SAMPLING_RADIUS): v = OpenRAVEViewer.create_viewer() bp = get_col_free_base_pose_around_target(t, plan, target_pose, pred.robot, save=True, dist=dist) v.draw_plan_ts(plan, t) attr_inds = OrderedDict() res = [] robot_attr_name_val_tuples = [('pose', bp)] add_to_attr_inds_and_res(t, attr_inds, res, pred.robot, robot_attr_name_val_tuples) return np.array(res), attr_inds
def test_resample_r_collides(self): domain, problem, params, plan = load_environment( "../domains/baxter_domain/baxter.domain", "../domains/baxter_domain/baxter_probs/baxter_test_env.prob") viewer = OpenRAVEViewer.create_viewer(plan.env) viewer.draw_plan_ts(plan, 0) baxter = plan.params['baxter'] obstacle = plan.params['table'] rcollides_pred = baxter_predicates.BaxterRCollides( "test_rcollides", [baxter, obstacle], ["Robot", "Obstacle"], plan.env) start_pose = np.array([0.4, 0.8, 1., 0.4, 0., 0., 0.]) end_pose = np.array([0.5, -0.881, 0.814, 1.669, -2.672, 0.864, 2.308]) traj = [] for i in range(7): traj.append( np.linspace(start_pose[i], end_pose[i], 40, endpoint=True)) traj = np.r_[traj] baxter.rArmPose = traj baxter.lArmPose = np.repeat(baxter.lArmPose[:, 0].reshape((7, 1)), 40, axis=1) baxter.rGripper = np.repeat(baxter.rGripper[:, 0].reshape((1, 1)), 40, axis=1) baxter.lGripper = np.repeat(baxter.lGripper[:, 0].reshape((1, 1)), 40, axis=1) baxter.pose = np.repeat(baxter.pose[:, 0].reshape((1, 1)), 40, axis=1) obstacle.pose = np.repeat(obstacle.pose[:, 0].reshape((3, 1)), 40, axis=1) obstacle.rotation = np.repeat(obstacle.rotation[:, 0].reshape((3, 1)), 40, axis=1) for i in range(1, 30): self.assertTrue(rcollides_pred.test(i))
def ee_reachable_resample(pred, negated, t, plan): assert not negated handles = [] v = OpenRAVEViewer.create_viewer() def target_trans_callback(target_trans): handles.append(plot_transform(v.env, target_trans)) v.draw_plan_ts(plan, t) def plot_time_step_callback(): v.draw_plan_ts(plan, t) plot_time_step_callback() targets = plan.get_param('GraspValid', 1, {0: pred.ee_pose}) assert len(targets) == 1 # confirm target is correct target_pose = targets[0].value[:, 0] set_robot_body_to_pred_values(pred, t) theta = 0 robot = pred.robot robot_body = pred._param_to_body[robot] for _ in range(NUM_EEREACHABLE_RESAMPLE_ATTEMPTS): # generate collision free base pose base_pose = get_col_free_base_pose_around_target( t, plan, target_pose, robot, save=True, dist=OBJ_RING_SAMPLING_RADIUS, callback=plot_time_step_callback) if base_pose is None: print "we should always be able to sample a collision-free base pose" st() # generate collision free arm pose target_rot = np.array([get_random_theta(), 0, 0]) torso_pose, arm_pose = get_col_free_torso_arm_pose( t, target_pose, target_rot, robot, robot_body, save=True, arm_pose_seed=None, callback=target_trans_callback) st() if torso_pose is None: print "we should be able to find an IK" continue # generate approach IK ee_trans = OpenRAVEBody.transform_from_obj_pose( target_pose, target_rot) rel_pt = pred.get_rel_pt(-pred._steps) target_pose_approach = np.dot(ee_trans, np.r_[rel_pt, 1])[:3] torso_pose_approach, arm_pose_approach = get_col_free_torso_arm_pose( t, target_pose_approach, target_rot, robot, robot_body, save=True, arm_pose_seed=arm_pose, callback=target_trans_callback) st() if torso_pose_approach is None: continue # generate retreat IK ee_trans = OpenRAVEBody.transform_from_obj_pose( target_pose, target_rot) rel_pt = pred.get_rel_pt(pred._steps) target_pose_retreat = np.dot(ee_trans, np.r_[rel_pt, 1])[:3] torso_pose_retreat, arm_pose_retreat = get_col_free_torso_arm_pose( t, target_pose_retreat, target_rot, robot, robot_body, save=True, arm_pose_seed=arm_pose, callback=target_trans_callback) st() if torso_pose_retreat is not None: break else: print "we should always be able to sample a collision-free base and arm pose" st() attr_inds = OrderedDict() res = [] arm_approach_traj = lin_interp_traj(arm_pose_approach, arm_pose, pred._steps) torso_approach_traj = lin_interp_traj(torso_pose_approach, torso_pose, pred._steps) base_approach_traj = lin_interp_traj(base_pose, base_pose, pred._steps) arm_retreat_traj = lin_interp_traj(arm_pose, arm_pose_retreat, pred._steps) torso_retreat_traj = lin_interp_traj(torso_pose, torso_pose_retreat, pred._steps) base_retreat_traj = lin_interp_traj(base_pose, base_pose, pred._steps) arm_traj = np.hstack((arm_approach_traj, arm_retreat_traj[:, 1:])) torso_traj = np.hstack((torso_approach_traj, torso_retreat_traj[:, 1:])) base_traj = np.hstack((base_approach_traj, base_retreat_traj[:, 1:])) # add attributes for approach and retreat for ind in range(2 * pred._steps + 1): robot_attr_name_val_tuples = [('rArmPose', arm_traj[:, ind]), ('backHeight', torso_traj[:, ind]), ('pose', base_traj[:, ind])] add_to_attr_inds_and_res(t + ind - pred._steps, attr_inds, res, pred.robot, robot_attr_name_val_tuples) st() ee_pose_attr_name_val_tuples = [('value', target_pose), ('rotation', target_rot)] add_to_attr_inds_and_res(t, attr_inds, res, pred.ee_pose, ee_pose_attr_name_val_tuples) # v.draw_plan_ts(plan, t) v.animate_range(plan, (t - pred._steps, t + pred._steps)) # check that indexes are correct # import ipdb; ipdb.set_trace() return np.array(res), attr_inds
def viewer(): return OpenRAVEViewer.create_viewer(plan.env)