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_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())
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))
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_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)
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))
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))
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 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
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
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))
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
def viewer():
return OpenRAVEViewer.create_viewer(plan.env)