def plan(self, start, goal):
        traj, cost = self.plan_with_inittraj(start, goal)
        if traj is not None:
            return traj, cost

        bounds = bound.get_bounds(self.robot, self.params.dof)
        solutions = []

        for i, waypoint in enumerate(self.gen_waypoint(bounds)):
            print '{} - int_planners try waypoint {} at depth {}...'.format(self.name(), i, self.params.depth)
            if self.verbose:
                utils.pv('waypoint')

            inittraj = []
            traj1, _ = self.int_planner1.plan(start, waypoint)
            if traj1 is not None:
                traj2, _ = self.int_planner2.plan(waypoint, goal)
                if traj2 is not None:
                    print '{} - int_planners find plans at depth {}...'.format(self.name(), self.params.depth)
                    if self.params.dof == 4:
                        traj1 = Planner.six_to_four([t for t in traj1])
                        traj2 = Planner.six_to_four([t for t in traj2])
                    inittraj.extend(traj1[0:self.int_planner1.params.n_steps] + traj2[-1-self.int_planner2.params.n_steps:])

                if inittraj:
                    traj, cost = self.plan_with_inittraj(start, goal, inittraj=inittraj)
                    if traj is not None:
                        solutions.append((traj, cost))
                        if self.params.first_return:
                            break

        if solutions:
            return sorted(solutions, key=lambda x: x[1])[0]

        return None, None
    def __init__(self, robot, params, verbose=False):
        super(Navigation, self).__init__(verbose)

        self.robot = robot
        self.params = params
        self.env = self.robot.GetEnv()
        self.bounds = bound.get_bounds(self.robot, 6)

        self.robot_state = state.State(self.env, verbose=self.verbose)
        self.planner = planner.create_planner(self.params.motion_planning.planner)(
            self.robot, self.params.motion_planning, self.verbose)
        self.simulator = simulator.Simulator(
            self.robot, self.robot_state, self.params.simulator, verbose=self.verbose)
Exemple #3
0
    def __init__(self, robot, params, verbose=False):
        super(Navigation, self).__init__(verbose)

        self.robot = robot
        self.params = params
        self.env = self.robot.GetEnv()
        self.bounds = bound.get_bounds(self.robot, 6)

        self.robot_state = state.State(self.env, verbose=self.verbose)
        self.planner = planner.create_planner(
            self.params.motion_planning.planner)(self.robot,
                                                 self.params.motion_planning,
                                                 self.verbose)
        self.simulator = simulator.Simulator(self.robot,
                                             self.robot_state,
                                             self.params.simulator,
                                             verbose=self.verbose)