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_namo_solver_one_move_plan_solve_init(self):
# return
plan = self.move_no_obs
# import ipdb; ipdb.set_trace()
move = plan.actions[0]
pr2 = move.params[0]
start = move.params[1]
end = move.params[2]
plan_params = plan.params.values()
for action in plan.actions:
for p in action.params:
self.assertTrue(p in plan_params)
for pred_dict in action.preds:
pred = pred_dict['pred']
for p in pred.params:
if p not in plan_params:
if pred_dict['hl_info'] != 'hl_state':
print pred
break
# self.assertTrue(p in plan_params)
callback = None
"""
Uncomment out lines below to see optimization.
"""
# viewer = OpenRAVEViewer()
# def callback():
# namo_solver._update_ll_params()
# viewer.draw_plan(plan)
# time.sleep(0.3)
"""
"""
namo_solver = ll_solver.NAMOSolver()
namo_solver._solve_opt_prob(plan, priority=-1, callback=callback)
namo_solver._update_ll_params()