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_backtrack_plan(test_obj,
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():
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.03)
elif method == 'Backtrack':
def callback(a):
solver._update_ll_params()
viewer.clear()
viewer.draw_plan_range(plan, a.active_timesteps)
time.sleep(0.3)
solver = can_solver.CanSolver(early_converge=early_converge)
start = time.time()
if method == 'SQP':
solver.solve(plan, callback=callback, verbose=verbose)
elif method == 'Backtrack':
solver.backtrack_solve(plan, callback=callback, verbose=verbose)
print "Solve Took: {}".format(time.time() - start)
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_plan(test_obj, plan, n_resamples=0):
print "testing plan: {}".format(plan.actions)
callback = None
viewer = None
"""
Uncomment out lines below to see optimization.
"""
viewer = test_obj.viewer
animate = get_animate_fn(viewer, plan)
draw_ts = get_draw_ts_fn(viewer, plan)
draw_cols_ts = get_draw_cols_ts_fn(viewer, plan)
def callback(set_trace=False):
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])
# draw_ts(50)
if set_trace:
animate()
# import ipdb; ipdb.set_trace()
# viewer.draw_plan(plan)
# time.sleep(0.03)
"""
"""
solver = can_solver.CanSolver()
solver.solve(plan,
callback=callback,
n_resamples=n_resamples,
verbose=False)
fp = plan.get_failed_preds()
_, _, t = plan.get_failed_pred()
#
if viewer != None:
if t < plan.horizon:
viewer.draw_plan_ts(plan, t)
def test_sample_ee_from_target(self):
solver = can_solver.CanSolver()
env = ParamSetup.setup_env()
# env.SetViewer('qtcoin')
target = ParamSetup.setup_target()
target.value = np.array([[0, 0, 0]]).T
target.rotation = np.array([[1.1, .3, 0]]).T
dummy_targ_geom = can.BlueCan(0.04, 0.25)
target_body = OpenRAVEBody(env, target.name, dummy_targ_geom)
target_body.set_pose(target.value.flatten(), target.rotation.flatten())
target_body.set_transparency(.7)
robot = ParamSetup.setup_pr2()
robot_body = OpenRAVEBody(env, robot.name, robot.geom)
robot_body.set_transparency(.7)
robot_body.set_pose(robot.pose.flatten())
dof_value_map = {
"backHeight": robot.backHeight,
"lArmPose": robot.lArmPose.flatten(),
"lGripper": robot.lGripper,
"rArmPose": robot.rArmPose.flatten(),
"rGripper": robot.rGripper
}
robot_body.set_dof(dof_value_map)
dummy_ee_pose_geom = can.GreenCan(.03, .3)
ee_list = list(
enumerate(
sampling.get_ee_from_target(target.value, target.rotation)))
for ee_pose in ee_list:
ee_pos, ee_rot = ee_pose[1]
body = OpenRAVEBody(env, "dummy" + str(ee_pose[0]),
dummy_ee_pose_geom)
body.set_pose(ee_pos, ee_rot)
body.set_transparency(.9)
def _test_resampling(test_obj, plan, n_resamples=0):
print "testing plan: {}".format(plan.actions)
callback = None
viewer = None
"""
Uncomment out lines below to see optimization.
"""
viewer = test_obj.viewer
animate = get_animate_fn(viewer, plan)
draw_ts = get_draw_ts_fn(viewer, plan)
draw_cols_ts = get_draw_cols_ts_fn(viewer, plan)
def callback(set_trace=False):
solver._update_ll_params()
# draw_ts(20)
# viewer.draw_plan(plan)
draw_ts(17)
# viewer.draw_cols_ts(plan, 17)
# time_range = (13,17)
# viewer.draw_plan_range(plan, time_range)
# viewer.draw_cols_range(plan, time_range)
# time.sleep(0.03)
# if set_trace:
# animate()
# import ipdb; ipdb.set_trace()
"""
"""
solver = can_solver.CanSolver()
# Initializing to sensible values
active_ts = (0, plan.horizon - 1)
verbose = False
solver._solve_opt_prob(plan,
priority=-2,
callback=callback,
active_ts=active_ts,
verbose=verbose)
solver._solve_opt_prob(plan,
priority=-1,
callback=callback,
active_ts=active_ts,
verbose=verbose)
for _ in range(n_resamples):
## refinement loop
## priority 0 resamples the first failed predicate in the plan
## and then solves a transfer optimization that only includes linear constraints
solver._solve_opt_prob(plan,
priority=0,
callback=callback,
active_ts=active_ts,
verbose=verbose)
fp = plan.get_failed_preds()
# import ipdb; ipdb.set_trace()
solver._solve_opt_prob(plan,
priority=1,
callback=calloback,
active_ts=active_ts,
verbose=verbose)
fp = plan.get_failed_preds()
# import ipdb; ipdb.set_trace()
success = solver._solve_opt_prob(plan,
priority=2,
callback=callback,
active_ts=active_ts,
verbose=verbose)
fp = plan.get_failed_preds()
# import ipdb; ipdb.set_trace()
if len(fp) == 0:
break
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(FAKE_TOL))