def check_obj_reachable(self, obj):
if len(self.problem_env.robot.GetGrabbed()) > 0:
return False
obj = self.problem_env.env.GetKinBody(obj)
obj_name = str(obj.GetName())
if self.problem_env.name.find('one_arm') != -1:
op = Operator('one_arm_pick', {'object': obj})
else:
op = Operator('two_arm_pick', {'object': obj})
if obj_name in self.pick_used:
motion_plan_goals = self.pick_used[obj_name].continuous_parameters['q_goal']
else:
motion_plan_goals = self.generate_potential_pick_configs(op, n_pick_configs=10)
if motion_plan_goals is not None:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
motion, status = motion_planner.get_motion_plan(motion_plan_goals)
is_feasible_param = status == 'HasSolution'
else:
is_feasible_param = False
if is_feasible_param:
op.make_pklable()
op.continuous_parameters = {'q_goal': motion[-1]}
self.motion_plans[obj_name] = motion
if obj_name not in self.pick_used:
self.pick_used[obj_name] = op
self.evaluations[obj_name] = True
return True
else:
self.evaluations[obj_name] = False
return False
def set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for obj, op_instance in self.pick_used.items():
motion_plan_goals = op_instance.continuous_parameters['q_goal']
self.cached_pick_paths[obj] = None
if not self.use_shortest_path:
continue
if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj:
self.cached_pick_paths[obj] = parent_state.cached_pick_paths[
obj]
else:
self.cached_pick_paths[obj] = motion_planner.get_motion_plan(
motion_plan_goals, cached_collisions={})[0]
if len(motion_plan_goals) == 0:
# This pick path is used for checking the pickinway predicate and to pickup the object in place in way predicate.
#
assert False
self.cached_pick_paths[obj] = None
else:
try:
# how can this be, since we disable all the collisions?
assert self.cached_pick_paths[obj] is not None
except:
import pdb
pdb.set_trace()
op_instance.continuous_parameters[
'potential_q_goals'] = motion_plan_goals
op_instance.continuous_parameters[
'q_goal'] = self.cached_pick_paths[obj][-1]
def get_minimum_constraint_path_to(self, goal_config, target_obj):
print "Planning to goal config:", goal_config
if self.use_shortest_path:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
else:
motion_planner = MinimumConstraintPlanner(self.problem_env, target_obj, 'prm')
motion, status = motion_planner.get_motion_plan(goal_config, cached_collisions=self.collides)
if motion is None:
return None, 'NoSolution'
return motion, status
def two_arm_domain_region_reachability_check(self, region):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
print "Motion planning to ", region
motion, status = motion_planner.get_motion_plan(self.problem_env.regions[region],
cached_collisions=self.collides)
if status == 'HasSolution':
self.motion_plans[region] = motion
self.evaluations[region] = True
return True
else:
self.evaluations[region] = False
return False
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
#self.reachable_regions_while_holding.append((obj, region_name))
else:
if self.holding_collides is not None:
path, status = motion_planner.get_motion_plan(region, cached_collisions=self.holding_collides)
else:
# I think the trouble here is that we do not hold the object when checking collisions
# So, the best way to solve this problem is to not keep reachable_regions_while_holding
# and just use the cached path. But I am wondering how we got a colliding-path in
# the first place. It must be from place_in_way? No, we execute this function first,
# and then using the cached paths, compute the place_in_way.
# Also, there is something wrong with the collision checking too.
# I think this has to do with the fact that the collisions are computed using
# the robot only, not with the object in hand.
# So, here is what I propose:
# Plan motions here, but do not add to reachable regions while holding.
# This way, it will plan motions as if it is not holding the object,
# but check collisions inside place_in_way
path, status = motion_planner.get_motion_plan(region,
cached_collisions=self.collisions_at_all_obj_pose_pairs)
if status == 'HasSolution':
pass
else:
obj_not_moved = obj != moved_obj
parent_state_has_cached_path_for_obj = parent_state is not None \
and obj in parent_state.cached_place_paths
# todo: What is this logic here...?
# it is about re-using the parent place path;
# but this assumes that the object has not moved?
if parent_state_has_cached_path_for_obj and obj_not_moved:
path = parent_state.cached_place_paths[(obj, region_name)]
else:
path, _ = motion_planner.get_motion_plan(region, cached_collisions={})
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
def approximate_minimal_collision_path(self, goal_configuration, path_ignoring_all_objects,
collisions_in_path_ignoring_all_objects, cached_collisions):
enabled_objects = {obj.GetName() for obj in self.problem_env.objects}
enabled_objects -= {obj.GetName() for obj in collisions_in_path_ignoring_all_objects}
[o.Enable(False) for o in collisions_in_path_ignoring_all_objects]
minimal_objects_in_way = []
minimal_collision_path = path_ignoring_all_objects
for obj in collisions_in_path_ignoring_all_objects:
obj.Enable(True)
[o.Enable(False) for o in minimal_objects_in_way]
enabled_objects.add(obj.GetName())
enabled_objects -= {obj.GetName() for obj in minimal_objects_in_way}
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration,
cached_collisions=cached_collisions)
else:
path, status = BaseMotionPlanner.get_motion_plan(self,
goal_configuration,
cached_collisions=cached_collisions,
n_iterations=[20, 50, 100])
if status != 'HasSolution':
minimal_objects_in_way.append(obj)
else:
minimal_collision_path = path
self.problem_env.enable_objects_in_region('entire_region')
return minimal_collision_path
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
parent_state_has_cached_path_for_obj \
= parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj
cached_path_is_shortest_path = parent_state is not None and \
not (obj, region_name) in parent_state.reachable_regions_while_holding
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
self.reachable_regions_while_holding.append((obj, region_name))
else:
if region.name == 'home_region':
# a location right at the entrance of home
goal = [np.array([0.73064842, -2.85306871, 4.87927762])]
else:
goal = region
if self.holding_collides is not None:
path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal,
cached_collisions=self.holding_collides,
return_start_set_and_path_idxs=True)
else:
# note: self.collides is computed without holding the object.
path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal,
cached_collisions=self.collides,
return_start_set_and_path_idxs=True)
if status == 'HasSolution':
self.reachable_regions_while_holding.append((obj, region_name))
else:
if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path:
path = parent_state.cached_place_paths[(obj, region_name)]
start_and_prm_idxs = parent_state.cached_place_start_and_prm_idxs[(obj, region_name)]
else:
path, _, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions={},
return_start_set_and_path_idxs=True)
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
self.cached_place_start_and_prm_idxs[(obj, region_name)] = start_and_prm_idxs
def add_trajectory(self, plan, goal_entities):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
motion_planner = BaseMotionPlanner(problem_env, 'prm')
problem_env.set_motion_planner(motion_planner)
idx = 0
parent_state = None
parent_action = None
moved_objs = {p.discrete_parameters['object'] for p in plan}
moved_obj_regions = {(p.discrete_parameters['object'],
p.discrete_parameters['region'])
for p in plan}
n_times_objs_moved = {obj_name: 0 for obj_name in moved_objs}
n_times_obj_region_moved = {
(obj_name, region_name): 0
for obj_name, region_name in moved_obj_regions
}
self.paps_used = self.get_pap_used_in_plan(plan)
curr_paps_used = self.account_for_used_picks_and_places(
n_times_objs_moved, n_times_obj_region_moved)
state = self.compute_state(parent_state, parent_action, goal_entities,
problem_env, curr_paps_used, 0)
for action_idx, action in enumerate(plan):
action.execute()
# mark that a pick or place in the plan has been used
target_obj_name = action.discrete_parameters['object']
target_region_name = action.discrete_parameters['region']
n_times_objs_moved[target_obj_name] += 1
n_times_obj_region_moved[(target_obj_name,
target_region_name)] += 1
curr_paps_used = self.account_for_used_picks_and_places(
n_times_objs_moved, n_times_obj_region_moved)
parent_state = state
parent_action = action
state = self.compute_state(parent_state, parent_action,
goal_entities, problem_env,
curr_paps_used, action_idx)
# execute the pap action
if action == plan[-1]:
reward = 0
else:
reward = -1
print "The reward is ", reward
self.add_sar_tuples(parent_state, action, reward)
print "Executed", action.discrete_parameters
self.add_state_prime()
openrave_env.Destroy()
openravepy.RaveDestroy()
def set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for obj, op_instance in self.pick_used.items():
motion_plan_goals = op_instance.continuous_parameters['q_goal']
assert len(motion_plan_goals) > 0
self.cached_pick_paths[obj] = None
path, status = motion_planner.get_motion_plan(
motion_plan_goals, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_entities.append(obj)
else:
path, _ = motion_planner.get_motion_plan(motion_plan_goals,
cached_collisions={})
assert path is not None
self.cached_pick_paths[obj] = path
op_instance.low_level_motion = path
def add_trajectory(self, plan, goal_entities):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
motion_planner = BaseMotionPlanner(problem_env, 'prm')
problem_env.set_motion_planner(motion_planner)
idx = 0
parent_state = None
parent_action = None
self.plan_sanity_check(problem_env, plan)
paps_used = self.get_pap_used_in_plan(plan)
pick_used = paps_used[0]
place_used = paps_used[1]
reward_function = ShapedRewardFunction(problem_env,
['square_packing_box1'],
'home_region', 3 * 8)
# utils.viewer()
state = self.compute_state(parent_state, parent_action, goal_entities,
problem_env, paps_used, 0)
for action_idx, action in enumerate(plan):
if 'place' in action.type:
continue
target_obj = openrave_env.GetKinBody(
action.discrete_parameters['object'])
color_before = get_color(target_obj)
set_color(target_obj, [1, 1, 1])
pick_used, place_used = self.delete_moved_objects_from_pap_data(
pick_used, place_used, target_obj)
paps_used = [pick_used, place_used]
action.is_skeleton = False
pap_action = copy.deepcopy(action)
pap_action = pap_action.merge_operators(plan[action_idx + 1])
pap_action.is_skeleton = False
pap_action.execute()
# set_color(target_obj, color_before)
parent_state = state
parent_action = pap_action
state = self.compute_state(parent_state, parent_action,
goal_entities, problem_env, paps_used,
action_idx)
# execute the pap action
reward = reward_function(parent_state, state, parent_action,
action_idx)
print "The reward is ", reward
self.add_sar_tuples(parent_state, pap_action, reward)
print "Executed", action.discrete_parameters
self.add_state_prime()
openrave_env.Destroy()
openravepy.RaveDestroy()
def set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
# making it consistent with the feasibility checker
self.problem_env.set_robot_to_default_dof_values()
for obj, op_instance in self.pick_used.items():
# print self.problem_env.robot.GetDOFValues()
motion_plan_goals = op_instance.continuous_parameters['q_goal']
assert len(motion_plan_goals) > 0
self.cached_pick_paths[obj] = None
path, status = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_entities.append(obj)
else:
path, _ = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions={})
[t.Enable(False) for t in self.problem_env.objects]
rrt_motion_planner = BaseMotionPlanner(self.problem_env, 'rrt')
# motion_plan_goals[0] = np.array([-0.11255534, -0.26290062, 1.64126379])
# utils.set_robot_config(motion_plan_goals[0])
for _ in range(100):
path, status = rrt_motion_planner.get_motion_plan(motion_plan_goals[0])
if status == 'HasSolution':
break
[t.Enable(True) for t in self.problem_env.objects]
assert path is not None, 'Even RRT failed!'
self.cached_pick_paths[obj] = path
op_instance.low_level_motion = path
def get_binary_edge_features(self, a, b):
if (a, b) in self.parent_binary_predicates:
return self.parent_binary_predicates[(a, b)]
else:
# todo rename cached_pick_path and cached_place_path as shortest paths
if self.use_shortest_path:
# todo this needs to be re-computed too when we are using shortest paths, because
# this is true if for all b in (a,b,r), b is not in the way of shortest path to r while holidng a
# Since the shortest path plans a path without collision-checking, this is not an accurate computation
if a in self.problem_env.object_names and b in self.problem_env.region_names and b != 'entire_region':
if a not in self.reachable_entities:
is_place_in_b_reachable_while_holding_a = False
else:
saver = CustomStateSaver(self.problem_env.env)
self.pick_used[a].execute(
) # it should not be in collision
motion_planner = BaseMotionPlanner(
self.problem_env, 'prm')
# note that it doesn't check the collision with the object held
plan, status = motion_planner.get_motion_plan(
self.problem_env.regions[b],
cached_collisions=self.collides)
saver.Restore()
is_place_in_b_reachable_while_holding_a = status == 'HasSolution'
else:
is_place_in_b_reachable_while_holding_a = False
else:
is_place_in_b_reachable_while_holding_a = (
a, b) in self.place_in_way.reachable_obj_region_pairs
if self.use_shortest_path:
if b.find('region') != -1:
cached_path = None
else:
cached_path = self.cached_pick_paths[b]
is_a_in_pick_path_of_b = self.pick_in_way(
a, b, cached_path=cached_path)
else:
is_a_in_pick_path_of_b = self.pick_in_way(a, b)
return [
self.in_region(a, b), is_a_in_pick_path_of_b,
is_place_in_b_reachable_while_holding_a
]
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
if not self.use_shortest_path:
continue
if pick_path is None:
continue
if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj:
self.cached_place_paths[(
obj, region_name)] = parent_state.cached_place_paths[(
obj, region_name)]
else:
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(
self.problem_env.env.GetKinBody(
obj).ComputeAABB()):
self.cached_place_paths[(obj, region_name)] = [
get_body_xytheta(self.problem_env.robot).squeeze()
]
else:
self.cached_place_paths[(
obj,
region_name)] = motion_planner.get_motion_plan(
region, cached_collisions={})[0]
if self.cached_place_paths[(obj, region_name)] is None:
import pdb
pdb.set_trace()
saver.Restore()
try:
assert self.cached_place_paths[(obj,
region_name)] is not None
except:
import pdb
pdb.set_trace()
def main():
problem_idx = 0
problem_env = Mover(problem_idx, problem_type='jobtalk')
problem_env.set_motion_planner(BaseMotionPlanner(problem_env, 'rrt'))
utils.viewer()
seed = 1
np.random.seed(seed)
random.seed(seed)
pick_smpler, place_smpler = create_sampler(problem_env)
# Set camera view - to get camera transform: viewer.GetCameraTransform()
cam_transform = np.array(
[[-0.54866337, -0.70682829, 0.44650004, -1.45953619],
[-0.83599448, 0.45806221, -0.30214604, 2.02016926],
[0.00904058, -0.53904803, -0.8422265, 4.88620949], [0., 0., 0., 1.]])
cam_transform = np.array(
[[0.76808539, 0.51022899, -0.38692533, 2.7075901],
[0.63937823, -0.57785198, 0.50723029, -2.0267117],
[0.03521803, -0.6369878, -0.77006898, 4.52542162], [0., 0., 0., 1.]])
init_goal_cam_transform = np.array(
[[0.99941927, -0.00186311, 0.03402425, 1.837726],
[-0.02526303, -0.71058334, 0.70315937, -5.78141165],
[0.022867, -0.70361058, -0.71021775, 6.03373909], [0., 0., 0., 1.]])
goal_obj_poses = pickle.load(
open('./test_scripts/jobtalk_figure_cache_files/goal_obj_poses.pkl',
'r'))
for o in problem_env.objects:
utils.set_obj_xytheta(goal_obj_poses[o.GetName()], o)
viewer = problem_env.env.GetViewer()
viewer.SetCamera(init_goal_cam_transform)
"""
# how do you go from intrinsic params to [fx, fy, cx, cy]? What are these anyways?
# cam_intrinsic_params = viewer.GetCameraIntrinsics()
I = problem_env.env.GetViewer().GetCameraImage(1920, 1080, cam_transform, cam_intrinsic_params)
scipy.misc.imsave('test.png', I)
"""
utils.set_obj_xytheta(np.array([0.01585576, 1.06516767, 5.77099297]),
target_obj_name)
pick_smpls = visualize_pick(pick_smpler, problem_env)
feasible_pick_op = get_feasible_pick(problem_env, pick_smpls)
feasible_pick_op.execute()
utils.visualize_placements(place_smpler(100),
problem_env.robot.GetGrabbed()[0])
import pdb
pdb.set_trace()
# Visualize path
"""
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
parent_state_has_cached_path_for_obj \
= parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj
cached_path_is_shortest_path = parent_state is not None and \
not (obj, region_name) in parent_state.reachable_regions_while_holding
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(
self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
self.reachable_regions_while_holding.append(
(obj, region_name))
else:
if self.holding_collides is not None:
path, status = motion_planner.get_motion_plan(
region, cached_collisions=self.holding_collides)
else:
path, status = motion_planner.get_motion_plan(
region, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_regions_while_holding.append(
(obj, region_name))
else:
if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path:
path = parent_state.cached_place_paths[(
obj, region_name)]
else:
path, _ = motion_planner.get_motion_plan(
region, cached_collisions={})
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
def compute_path_ignoring_obstacles(self, goal_configuration):
self.problem_env.disable_objects_in_region('entire_region')
if self.target_object is not None:
self.target_object.Enable(True)
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration)
else:
stime = time.time()
path, status = BaseMotionPlanner.get_motion_plan(self, goal_configuration)
print "Motion plan time", time.time()-stime
self.problem_env.enable_objects_in_region('entire_region')
if path is None:
import pdb; pdb.set_trace()
return path
def get_node_features(self, entity, goal_entities):
isobj = entity not in self.problem_env.regions
obj = self.problem_env.env.GetKinBody(entity) if isobj else None
pose = get_body_xytheta(obj)[0] if isobj else None
if isobj:
if self.use_shortest_path:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
pick_for_obj = self.pick_used[obj.GetName()]
plan, status = motion_planner.get_motion_plan(
pick_for_obj.continuous_parameters['potential_q_goals'],
cached_collisions=self.collides)
pick_for_obj.low_level_motion = plan
if status == 'HasSolution':
pick_for_obj.continuous_parameters['q_goal'] = plan[-1]
self.reachable_entities.append(entity)
is_entity_reachable = True
else:
is_entity_reachable = False
else:
is_entity_reachable = obj.GetName() in self.reachable_entities
else:
is_entity_reachable = True
return [
0, # l
0, # w
0, # h
pose[0] if isobj else 0, # x
pose[1] if isobj else 0, # y
pose[2] if isobj else 0, # theta
entity not in self.problem_env.regions, # IsObj
entity in self.problem_env.regions, # IsRoom
entity in self.goal_entities, # IsGoal
is_entity_reachable,
self.is_holding_goal_entity(),
]
def approximate_minimal_collision_path(
self, goal_configuration, path_ignoring_all_objects,
collisions_in_path_ignoring_all_objects, cached_collisions):
# enabled objects = all the objects except the ones that are in collision
enabled_objects = {obj.GetName() for obj in self.problem_env.objects}
enabled_objects -= {
obj.GetName()
for obj in collisions_in_path_ignoring_all_objects
}
[o.Enable(False) for o in collisions_in_path_ignoring_all_objects]
# enable object one by one. If we can find a path with it turned off, then it is not actually in the way.
# minimal objects in way = a set of objects that we cannot ignore
# for other objects in collisions_in_path_ignoring_all_objects, we can ignore
minimal_objects_in_way = []
minimal_collision_path = path_ignoring_all_objects
print "Approximating MCR path..."
for obj in collisions_in_path_ignoring_all_objects:
obj.Enable(True)
[o.Enable(False) for o in minimal_objects_in_way]
enabled_objects.add(obj.GetName())
enabled_objects -= {
obj.GetName()
for obj in minimal_objects_in_way
}
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(
self,
goal_configuration,
cached_collisions=cached_collisions)
else:
path, status = BaseMotionPlanner.get_motion_plan(
self,
goal_configuration,
cached_collisions=cached_collisions,
n_iterations=[20, 50, 100, 500, 1000])
if status != 'HasSolution':
minimal_objects_in_way.append(obj)
else:
minimal_collision_path = path
self.problem_env.enable_objects_in_region('entire_region')
return minimal_collision_path
def execute_policy(plan, problem_env, goal_entities, config):
try:
init_abstract_state = pickle.load(open('temp111.pkl', 'r'))
except:
init_abstract_state = make_abstract_state(problem_env, goal_entities)
init_abstract_state.make_pklable()
pickle.dump(init_abstract_state, open('temp111.pkl', 'wb'))
init_abstract_state.make_plannable(problem_env)
abstract_state = init_abstract_state
total_ik_checks = 0
total_mp_checks = 0
total_pick_mp_checks = 0
total_place_mp_checks = 0
total_pick_mp_infeasible = 0
total_place_mp_infeasible = 0
total_infeasible_mp = 0
plan_idx = 0
n_total_actions = 0
goal_reached = False
stime = time.time()
samples_tried = {i: [] for i in range(len(plan))}
sample_values = {i: [] for i in range(len(plan))}
learned_sampler_model = get_learned_sampler_models(config)
problem_env.set_motion_planner(BaseMotionPlanner(problem_env, 'rrt'))
while plan_idx < len(plan):
goal_reached = problem_env.is_goal_reached()
if goal_reached:
break
if n_total_actions >= 100:
break
action = plan[plan_idx]
generator = get_generator(abstract_state, action,
learned_sampler_model, config)
if 'two_arm' in config.domain:
cont_smpl = generator.sample_next_point(samples_tried[plan_idx],
sample_values[plan_idx])
else:
pick_smpl, place_smpl, status = generator.sample_next_point(
samples_tried[plan_idx], sample_values[plan_idx])
cont_smpl = {
'pick': pick_smpl,
'place': place_smpl,
'is_feasible': status == "HasSolution"
}
total_ik_checks += generator.n_ik_checks
total_mp_checks += generator.n_mp_checks
total_infeasible_mp += generator.n_mp_infeasible
total_pick_mp_checks += generator.n_pick_mp_checks
total_place_mp_checks += generator.n_place_mp_checks
total_pick_mp_infeasible += generator.n_pick_mp_infeasible
total_place_mp_infeasible += generator.n_place_mp_infeasible
n_total_actions += 1
if cont_smpl['is_feasible']:
print "Action executed"
action.continuous_parameters = cont_smpl
action.execute()
import pdb
pdb.set_trace()
plan_idx += 1
abstract_state = make_abstract_state(problem_env,
goal_entities,
parent_state=abstract_state,
parent_action=action)
else:
print "No feasible action"
problem_env.init_saver.Restore()
plan_idx = 0
"""
for s in cont_smpl['samples']:
samples_tried[plan_idx].append(s)
sample_values[plan_idx].append(generator.sampler.infeasible_action_value)
"""
abstract_state = init_abstract_state
goal_reached = plan_idx == len(plan)
print "Total IK checks {} Total actions {}".format(
total_ik_checks, n_total_actions)
import pdb
pdb.set_trace()
print time.time() - stime
return total_ik_checks, total_pick_mp_checks, total_pick_mp_infeasible, total_place_mp_checks, \
total_place_mp_infeasible, total_mp_checks, total_infeasible_mp, n_total_actions, goal_reached
def create_environment(problem_idx):
problem_env = Mover(problem_idx)
openrave_env = problem_env.env
problem_env.set_motion_planner(BaseMotionPlanner(problem_env, 'prm'))
return problem_env, openrave_env
def main():
parameters = parse_parameters()
save_dir = make_and_get_save_dir(parameters)
file_path = save_dir + '/seed_' + str(parameters.planner_seed) + '_pidx_' + str(parameters.pidx) + '.pkl'
quit_if_already_tested(file_path, parameters.f)
# for creating problem
np.random.seed(parameters.pidx)
random.seed(parameters.pidx)
is_two_arm_env = parameters.domain.find('two_arm') != -1
if is_two_arm_env:
environment = Mover(parameters.pidx)
else:
environment = OneArmMover(parameters.pidx)
environment.initial_robot_base_pose = get_body_xytheta(environment.robot)
if parameters.domain == 'two_arm_mover':
goal_region = 'home_region'
if parameters.n_objs_pack == 4:
goal_object_names = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3',
'rectangular_packing_box4']
else:
goal_object_names = ['square_packing_box1']
environment.set_goal(goal_object_names, goal_region)
elif parameters.domain == 'one_arm_mover':
goal_region = 'rectangular_packing_box1_region'
assert parameters.n_objs_pack == 1
goal_object_names = ['c_obst1']
environment.set_goal(goal_object_names, goal_region)
goal_entities = goal_object_names + [goal_region]
# for randomized algorithms
np.random.seed(parameters.planner_seed)
random.seed(parameters.planner_seed)
if parameters.v:
utils.viewer()
environment.set_motion_planner(BaseMotionPlanner(environment, 'rrt'))
# from manipulation.bodies.bodies import set_color
# set_color(environment.env.GetKinBody(goal_object_names[0]), [1, 0, 0])
start_time = time.time()
n_mp = n_ik = 0
goal_object_names, high_level_plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time,
parameters) # finds the plan
total_time_taken = time.time() - start_time
n_mp += mp
n_ik += ik
total_n_nodes = 0
total_plan = []
idx = 0
found_solution = False
timelimit = parameters.timelimit
while total_time_taken < timelimit:
goal_obj_name = goal_object_names[idx]
plan, n_nodes, status, (mp, ik) = find_plan_for_obj(goal_obj_name, high_level_plan[idx], environment,
start_time,
timelimit, parameters)
total_n_nodes += n_nodes
total_time_taken = time.time() - start_time
print goal_obj_name, goal_object_names, total_n_nodes
print "Time taken: %.2f" % total_time_taken
if total_time_taken > timelimit:
break
if status == 'HasSolution':
execute_plan(plan)
environment.initial_robot_base_pose = utils.get_body_xytheta(environment.robot)
total_plan += plan
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
idx += 1
else:
# Note that HPN does not have any recourse if this happens. We re-plan at the higher level.
goal_object_names, plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time, parameters) # finds the plan
n_mp += mp
n_ik += ik
total_plan = []
idx = 0
if idx == len(goal_object_names):
found_solution = True
break
else:
idx %= len(goal_object_names)
total_time_taken = time.time() - start_time
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
print 'plan saved'
def set_problem_env_config(problem_env, config):
np.random.seed(config.planner_seed)
random.seed(config.planner_seed)
problem_env.set_motion_planner(BaseMotionPlanner(problem_env, 'prm'))
problem_env.seed = config.pidx
problem_env.init_saver = DynamicEnvironmentStateSaver(problem_env.env)
def generate_training_data_single():
np.random.seed(config.pidx)
random.seed(config.pidx)
if config.domain == 'two_arm_mover':
mover = Mover(config.pidx, config.problem_type)
elif config.domain == 'one_arm_mover':
mover = OneArmMover(config.pidx)
else:
raise NotImplementedError
np.random.seed(config.planner_seed)
random.seed(config.planner_seed)
mover.set_motion_planner(BaseMotionPlanner(mover, 'prm'))
mover.seed = config.pidx
# todo change the root node
mover.init_saver = DynamicEnvironmentStateSaver(mover.env)
hostname = socket.gethostname()
if hostname in {'dell-XPS-15-9560', 'phaedra', 'shakey', 'lab', 'glaucus', 'luke-laptop-1'}:
root_dir = './'
#root_dir = '/home/beomjoon/Dropbox (MIT)/cloud_results/'
else:
root_dir = '/data/public/rw/pass.port/tamp_q_results/'
solution_file_dir = root_dir + '/test_results/greedy_results_on_mover_domain/domain_%s/n_objs_pack_%d'\
% (config.domain, config.n_objs_pack)
if config.dont_use_gnn:
solution_file_dir += '/no_gnn/'
elif config.dont_use_h:
solution_file_dir += '/gnn_no_h/loss_' + str(config.loss) + '/num_train_' + str(config.num_train) + '/'
elif config.hcount:
solution_file_dir += '/hcount/'
elif config.hadd:
solution_file_dir += '/gnn_hadd/loss_' + str(config.loss) + '/num_train_' + str(config.num_train) + '/'
else:
solution_file_dir += '/gnn/loss_' + str(config.loss) + '/num_train_' + str(config.num_train) + '/'
solution_file_name = 'pidx_' + str(config.pidx) + \
'_planner_seed_' + str(config.planner_seed) + \
'_train_seed_' + str(config.train_seed) + \
'_domain_' + str(config.domain) + '.pkl'
if not os.path.isdir(solution_file_dir):
os.makedirs(solution_file_dir)
solution_file_name = solution_file_dir + solution_file_name
is_problem_solved_before = os.path.isfile(solution_file_name)
if is_problem_solved_before and not config.plan:
with open(solution_file_name, 'rb') as f:
trajectory = pickle.load(f)
success = trajectory.metrics['success']
tottime = trajectory.metrics['tottime']
else:
t = time.time()
trajectory, num_nodes = get_problem(mover)
tottime = time.time() - t
success = trajectory is not None
plan_length = len(trajectory.actions) if success else 0
if not success:
trajectory = Trajectory(mover.seed, mover.seed)
trajectory.states = [s.get_predicate_evaluations() for s in trajectory.states]
trajectory.state_prime = None
trajectory.metrics = {
'n_objs_pack': config.n_objs_pack,
'tottime': tottime,
'success': success,
'plan_length': plan_length,
'num_nodes': num_nodes,
}
#with open(solution_file_name, 'wb') as f:
# pickle.dump(trajectory, f)
print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, trajectory.metrics['plan_length'],
trajectory.metrics['num_nodes'])
import pdb;pdb.set_trace()
"""
print("time: {}".format(','.join(str(trajectory.metrics[m]) for m in [
'n_objs_pack',
'tottime',
'success',
'plan_length',
'num_nodes',
])))
"""
print('\n'.join(str(a.discrete_parameters.values()) for a in trajectory.actions))
def draw_robot_line(env, q1, q2):
return draw_line(env, list(q1)[:2] + [.5], list(q2)[:2] + [.5], color=(0,0,0,1), width=3.)
mover.reset_to_init_state_stripstream()
if not config.dontsimulate:
if config.visualize_sim:
mover.env.SetViewer('qtcoin')
set_viewer_options(mover.env)
raw_input('Start?')
handles = []
for step_idx, action in enumerate(trajectory.actions):
if action.type == 'two_arm_pick_two_arm_place':
def check_collisions(q=None):
if q is not None:
set_robot_config(q, mover.robot)
collision = False
if mover.env.CheckCollision(mover.robot):
collision = True
for obj in mover.objects:
if mover.env.CheckCollision(obj):
collision = True
if collision:
print('collision')
if config.visualize_sim:
raw_input('Continue after collision?')
check_collisions()
o = action.discrete_parameters['two_arm_place_object']
pick_params = action.continuous_parameters['pick']
place_params = action.continuous_parameters['place']
full_path = [get_body_xytheta(mover.robot)[0]] + pick_params['motion'] + [pick_params['q_goal']] + \
place_params['motion'] + [place_params['q_goal']]
for i, (q1, q2) in enumerate(zip(full_path[:-1], full_path[1:])):
if np.linalg.norm(np.squeeze(q1)[:2] - np.squeeze(q2)[:2]) > 1:
print(i, q1, q2)
import pdb;
pdb.set_trace()
pickq = pick_params['q_goal']
pickt = pick_params['motion']
check_collisions(pickq)
for q in pickt:
check_collisions(q)
obj = mover.env.GetKinBody(o)
if len(pickt) > 0:
for q1, q2 in zip(pickt[:-1], pickt[1:]):
handles.append(
draw_robot_line(mover.env, q1.squeeze(), q2.squeeze()))
# set_robot_config(pickq, mover.robot)
# if config.visualize_sim:
# raw_input('Continue?')
# set_obj_xytheta([1000,1000,0], obj)
two_arm_pick_object(obj, pick_params)
if config.visualize_sim:
raw_input('Place?')
o = action.discrete_parameters['two_arm_place_object']
r = action.discrete_parameters['two_arm_place_region']
placeq = place_params['q_goal']
placep = place_params['object_pose']
placet = place_params['motion']
check_collisions(placeq)
for q in placet:
check_collisions(q)
obj = mover.env.GetKinBody(o)
if len(placet) > 0:
for q1, q2 in zip(placet[:-1], placet[1:]):
handles.append(
draw_robot_line(mover.env, q1.squeeze(), q2.squeeze()))
# set_robot_config(placeq, mover.robot)
# if config.visualize_sim:
# raw_input('Continue?')
# set_obj_xytheta(placep, obj)
two_arm_place_object(place_params)
check_collisions()
if config.visualize_sim:
raw_input('Continue?')
elif action.type == 'one_arm_pick_one_arm_place':
def check_collisions(q=None):
if q is not None:
set_robot_config(q, mover.robot)
collision = False
if mover.env.CheckCollision(mover.robot):
collision = True
for obj in mover.objects:
if mover.env.CheckCollision(obj):
collision = True
if collision:
print('collision')
if config.visualize_sim:
raw_input('Continue after collision?')
check_collisions()
pick_params = action.continuous_parameters['pick']
place_params = action.continuous_parameters['place']
one_arm_pick_object(mover.env.GetKinBody(action.discrete_parameters['object']), pick_params)
check_collisions()
if config.visualize_sim:
raw_input('Place?')
one_arm_place_object(place_params)
check_collisions()
if config.visualize_sim:
raw_input('Continue?')
else:
raise NotImplementedError
n_objs_pack = config.n_objs_pack
if config.domain == 'two_arm_mover':
goal_region = 'home_region'
elif config.domain == 'one_arm_mover':
goal_region = 'rectangular_packing_box1_region'
else:
raise NotImplementedError
if all(mover.regions[goal_region].contains(o.ComputeAABB()) for o in
mover.objects[:n_objs_pack]):
print("successful plan length: {}".format(len(trajectory.actions)))
else:
print('failed to find plan')
if config.visualize_sim:
raw_input('Finish?')
return
def __init__(self, problem_env, target_object, planning_algorithm):
BaseMotionPlanner.__init__(self, problem_env, planning_algorithm)
if type(target_object) == str:
self.target_object = self.problem_env.env.GetKinBody(target_object)
else:
self.target_object = target_object
