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)
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)