Ejemplo n.º 1
0
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))
Ejemplo n.º 2
0
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())
Ejemplo n.º 3
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))
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
    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)
Ejemplo n.º 6
0
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))
Ejemplo n.º 7
0
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))
Ejemplo n.º 8
0
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)
Ejemplo n.º 9
0
    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
Ejemplo n.º 10
0
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
Ejemplo n.º 11
0
 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))
Ejemplo n.º 12
0
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
Ejemplo n.º 13
0
 def viewer():
     return OpenRAVEViewer.create_viewer(plan.env)