def __init__(self, problem_idx, problem_type='normal'):
ProblemEnvironment.__init__(self, problem_idx)
problem_defn = MoverEnvironmentDefinition(self.env)
self.problem_config = problem_defn.get_problem_config()
self.robot = self.env.GetRobots()[0]
self.objects = self.problem_config['packing_boxes']
self.set_problem_type(problem_type)
self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
self.initial_robot_base_pose = get_body_xytheta(self.robot)
self.regions = {'entire_region': self.problem_config['entire_region'],
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']}
self.region_names = ['entire_region', 'home_region', 'loading_region']
self.object_names = [obj.GetName() for obj in self.objects]
self.placement_regions = {'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']}
#self.entity_names = self.object_names + ['home_region', 'loading_region','entire_region']
self.entity_names = self.object_names + ['home_region', 'loading_region', 'entire_region']
self.entity_idx = {name: idx for idx, name in enumerate(self.entity_names)}
self.is_init_pick_node = True
self.name = 'two_arm_mover'
self.init_saver = CustomStateSaver(self.env)
self.problem_config['env'] = self.env
self.operator_names = ['two_arm_pick', 'two_arm_place']
self.reward_function = None
self.applicable_op_constraint = None
self.two_arm_pick_continuous_constraint = None
self.two_arm_place_continuous_constraint = None
self.objects_to_check_collision = None
self.goal = None
def update_collisions_at_prm_vertices(self, parent_collides):
# global prm_vertices
# global prm_edges
# if prm_vertices is None or prm_edges is None:
# prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
is_robot_holding = len(self.problem_env.robot.GetGrabbed()) > 0
def in_collision(q, obj):
set_robot_config(q, self.problem_env.robot)
if is_robot_holding:
# note:
# openrave bug: when an object is held, it won't check the held_obj and given object collision unless
# collision on robot is first checked. So, we have to check it twice
col = self.problem_env.env.CheckCollision(
self.problem_env.robot)
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
else:
col = self.problem_env.env.CheckCollision(
self.problem_env.robot, obj)
return col
obj_name_to_pose = {
obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6))
for obj in self.problem_env.objects
}
collisions_at_all_obj_pose_pairs = {}
old_q = get_body_xytheta(self.problem_env.robot)
for obj in self.problem_env.objects:
obj_name_pose_tuple = (obj.GetName(),
obj_name_to_pose[obj.GetName()])
collisions_with_obj_did_not_change = parent_collides is not None and \
obj_name_pose_tuple in parent_collides
if collisions_with_obj_did_not_change:
collisions_at_all_obj_pose_pairs[
obj_name_pose_tuple] = parent_collides[obj_name_pose_tuple]
else:
prm_vertices_in_collision_with_obj = {
i
for i, q in enumerate(self.prm_vertices)
if in_collision(q, obj)
}
collisions_at_all_obj_pose_pairs[
obj_name_pose_tuple] = prm_vertices_in_collision_with_obj
set_robot_config(old_q, self.problem_env.robot)
# what's the diff between collides and curr collides?
# collides include entire set of (obj, obj_pose) pairs that we have seen so far
# current collides are the collisions at the current object pose
collisions_at_current_obj_pose_pairs = {
obj.GetName():
collisions_at_all_obj_pose_pairs[(obj.GetName(),
obj_name_to_pose[obj.GetName()])]
for obj in self.problem_env.objects
}
return collisions_at_all_obj_pose_pairs, collisions_at_current_obj_pose_pairs
def update_collisions_at_prm_vertices(self, parent_state):
global prm_vertices
global prm_edges
if prm_vertices is None or prm_edges is None:
prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb'))
holding = len(self.problem_env.robot.GetGrabbed()) > 0
if holding:
held = self.problem_env.robot.GetGrabbed()[0]
def in_collision(q, obj):
#old_q = get_body_xytheta(self.problem_env.robot)
set_robot_config(q, self.problem_env.robot)
col = self.problem_env.env.CheckCollision(self.problem_env.robot,
obj)
#set_robot_config(old_q, self.problem_env.robot)
return col
obj_name_to_pose = {
obj.GetName(): tuple(get_body_xytheta(obj)[0].round(6))
for obj in self.problem_env.objects
}
# if robot is holding an object, all collision information changes
# but we should still retain previous collision information
# because collisions at the next state will be similar to collisions at the previous state
# todo once we placed the object, instead of setting the collides to be empty,
# we could use the collision information from state-before-pick, because
# the robot shape goes back to whatever it was.
collides = copy.copy(parent_state.collides) if holding else {}
old_q = get_body_xytheta(self.problem_env.robot)
for obj in self.problem_env.objects:
obj_name_pose_tuple = (obj.GetName(),
obj_name_to_pose[obj.GetName()])
collisions_with_obj_did_not_change = parent_state is not None and \
obj_name_pose_tuple in parent_state.collides and \
not holding
if collisions_with_obj_did_not_change:
collides[obj_name_pose_tuple] = parent_state.collides[
obj_name_pose_tuple]
else:
prm_vertices_in_collision_with_obj = {
i
for i, q in enumerate(prm_vertices)
if in_collision(q, obj)
}
collides[
obj_name_pose_tuple] = prm_vertices_in_collision_with_obj
set_robot_config(old_q, self.problem_env.robot)
current_collides = {
obj.GetName():
collides[(obj.GetName(), obj_name_to_pose[obj.GetName()])]
for obj in self.problem_env.objects
}
return collides, current_collides
def __init__(self, abstract_state, abstract_action):
ConcreteNodeState.__init__(self, abstract_state, abstract_action)
self.key_configs = self.abstract_state.prm_vertices
self.pick_collision_vector = self.get_konf_obstacles(abstract_state.current_collides)
self.place_collision_vector = None
self.abs_robot_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.robot))
self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.env.GetKinBody(self.obj)))
self.abs_goal_obj_poses = [np.array([0, 0, 0])]
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.parent = parent_state # used to update the node features
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
def get_pick_poses(self, object, moved_obj, parent_state):
if parent_state is not None and moved_obj != object.GetName():
return parent_state.pick_used[object.GetName()]
operator_skeleton = Operator('two_arm_pick', {'object': object})
generator = UniformGenerator(operator_skeleton, self.problem_env, None)
# This is just generating pick poses. It does not check motion feasibility
self.problem_env.disable_objects_in_region('entire_region')
object.Enable(True)
self.problem_env.set_robot_to_default_dof_values()
n_iters = range(10, 500, 10)
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, None)
if len(feasible_cont_params) == 0 and moved_obj == object.GetName():
given_base_pose = utils.get_robot_xytheta()
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, given_base_pose)
orig_xytheta = get_body_xytheta(self.problem_env.robot)
self.problem_env.enable_objects_in_region('entire_region')
min_n_collisions = np.inf
chosen_pick_param = None
for cont_param in feasible_cont_params:
n_collisions = self.problem_env.get_n_collisions_with_objects_at_given_base_conf(cont_param['q_goal'])
if min_n_collisions > n_collisions:
chosen_pick_param = cont_param
min_n_collisions = n_collisions
utils.set_robot_config(orig_xytheta)
# assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable
operator_skeleton.continuous_parameters = chosen_pick_param
operator_skeleton.continuous_parameters['q_goal'] = [chosen_pick_param['q_goal']]
return operator_skeleton
def sample_from_continuous_space(self):
assert len(self.robot.GetGrabbed()) == 0
# sample pick
pick_cont_params = None
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
robot_pose = utils.get_body_xytheta(self.robot)
robot_config = self.robot.GetDOFValues()
# sample pick parameters
pick_cont_params, status = self.sample_pick_cont_parameters()
if status != 'HasSolution':
utils.set_robot_config(robot_pose)
return None, None, status
self.pick_op.continuous_parameters = pick_cont_params
self.pick_op.execute()
# sample place
place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params)
# reverting back to the state before sampling
utils.one_arm_place_object(pick_cont_params)
self.robot.SetDOFValues(robot_config)
utils.set_robot_config(robot_pose)
if status != 'HasSolution':
return None, None, "InfeasibleIK"
else:
self.place_op.continuous_parameters = place_cont_params
return pick_cont_params, place_cont_params, status
def sample_from_continuous_space(self):
# sample pick
pick_cont_params = None
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
robot_pose = utils.get_body_xytheta(self.robot)
robot_config = self.robot.GetDOFValues()
# sample pick parameters
while pick_cont_params is None:
pick_cont_params, status = self.sample_pick_cont_parameters()
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 4:
break
if status != 'HasSolution':
utils.set_robot_config(robot_pose)
return None, None, status
self.pick_op.continuous_parameters = pick_cont_params
self.pick_op.execute()
# sample place
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
place_cont_params = None
while place_cont_params is None:
place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params)
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 1:
break
# reverting back to the state before sampling
utils.one_arm_place_object(pick_cont_params)
self.robot.SetDOFValues(robot_config)
utils.set_robot_config(robot_pose)
if status != 'HasSolution':
return None, None, status
else:
self.place_op.continuous_parameters = place_cont_params
return pick_cont_params, place_cont_params, status
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
paps_used_in_data=None):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.problem_env.set_robot_to_default_dof_values()
self.parent_state = parent_state # used to update the node features
self.goal_entities = goal_entities
self.object_names = [str(obj.GetName()) for obj in problem_env.objects]
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
self.use_prm = problem_env.name.find('two_arm') != -1
if paps_used_in_data is not None:
self.pick_used = paps_used_in_data[0]
self.place_used = paps_used_in_data[1]
self.mc_pick_path = {}
self.mc_place_path = {}
self.reachable_entities = []
self.pick_in_way = None
self.place_in_way = None
self.in_region = None
self.is_holding_goal_entity = None
self.ternary_edges = None
self.binary_edges = None
self.nodes = None
self.prm_vertices, self.prm_edges = pickle.load(open(
'./prm.pkl', 'rb'))
def get_smpler_state(pidx, obj, problem_env):
fname = cached_env_path + 'policy_eval_pidx_%d.pkl' % pidx
if os.path.isfile(fname):
state = pickle.load(open(fname, 'r'))
else:
state = compute_state(obj, 'loading_region', problem_env)
pickle.dump(state, open(fname, 'wb'))
state.obj = obj
state.goal_flags = state.get_goal_flags()
state.abs_obj_pose = utils.get_body_xytheta(obj)
return state
def get_place_param_with_feasible_motion_plan(self, chosen_pick_param,
candidate_pap_parameters,
cached_holding_collisions):
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
utils.two_arm_pick_object(
self.operator_skeleton.discrete_parameters['object'],
chosen_pick_param)
place_op_params = [op['place'] for op in candidate_pap_parameters]
chosen_place_param = self.get_op_param_with_feasible_motion_plan(
place_op_params, cached_holding_collisions)
utils.two_arm_place_object(chosen_pick_param)
utils.set_robot_config(original_config)
return chosen_place_param
def __init__(self, abstract_state, abstract_action, key_configs, parent_state=None):
ConcreteNodeState.__init__(self, abstract_state, abstract_action)
self.parent_state = parent_state
self.key_configs = key_configs
init_config = utils.get_rightarm_torso_config(abstract_state.problem_env.robot)
self.pick_collision_vector = self.get_konf_obstacles_from_key_configs()
#self.konf_collisions_with_obj_poses = self.get_object_pose_collisions()
#self.pick_collision_vector = self.get_konf_obstacles(self.konf_collisions_with_obj_poses)
utils.set_rightarm_torso(init_config, abstract_state.problem_env.robot)
self.place_collision_vector = None
# following three variables are used in get_processed_poses_from_state
self.abs_robot_pose = utils.get_rightarm_torso_config(self.problem_env.robot)
self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(self.problem_env.env.GetKinBody(self.obj)))
self.abs_goal_obj_poses = [np.array([0, 0, 0])]
def __init__(self,
problem_env,
obj,
region,
goal_entities,
key_configs=None,
collision_vector=None):
self.obj = obj
self.region = region
self.goal_entities = goal_entities
self.key_configs = self.get_key_configs(key_configs)
self.collision_vector = self.get_collison_vector(collision_vector)
if type(obj) == str or type(obj) == unicode:
obj = problem_env.env.GetKinBody(obj)
self.abs_robot_pose = utils.clean_pose_data(
utils.get_body_xytheta(problem_env.robot))
self.abs_obj_pose = utils.clean_pose_data(utils.get_body_xytheta(obj))
self.abs_goal_obj_pose = utils.clean_pose_data(
utils.get_body_xytheta('square_packing_box1'))
self.goal_flags = self.get_goal_flags()
self.rel_konfs = None
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 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 get_augmented_state_vec_and_poses(obj, state_vec, smpler_state):
n_key_configs = 615
utils.set_color(obj, [1, 0, 0])
is_goal_obj = utils.convert_binary_vec_to_one_hot(
np.array([obj in smpler_state.goal_entities]))
is_goal_obj = np.tile(is_goal_obj, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
is_goal_region = utils.convert_binary_vec_to_one_hot(
np.array([smpler_state.region in smpler_state.goal_entities]))
is_goal_region = np.tile(is_goal_region, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
state_vec = np.concatenate([state_vec, is_goal_obj, is_goal_region],
axis=2)
poses = np.hstack([
utils.encode_pose_with_sin_and_cos_angle(
utils.get_body_xytheta(obj).squeeze()), 0, 0, 0, 0
]).reshape((1, 8))
return state_vec, poses
def check_place_feasible(self, pick_parameters, place_parameters,
operator_skeleton):
pick_op = Operator('two_arm_pick',
operator_skeleton.discrete_parameters)
pick_op.continuous_parameters = pick_parameters
# todo remove the CustomStateSaver
#saver = utils.CustomStateSaver(self.problem_env.env)
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
pick_op.execute()
place_op = Operator('two_arm_place',
operator_skeleton.discrete_parameters)
place_cont_params, place_status = TwoArmPlaceFeasibilityChecker.check_feasibility(
self, place_op, place_parameters)
utils.two_arm_place_object(pick_op.continuous_parameters)
utils.set_robot_config(original_config)
#saver.Restore()
return place_cont_params, place_status
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 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
is_entity_reachable = self.is_reachable(entity)
# todo turn these into one-hot encoding
# todo I need is_holding predicate for not putting high value on regions
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 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 get_object_pose_collisions(self):
def in_collision_with_obj(q, obj):
utils.set_rightarm_torso(q, self.problem_env.robot)
col = self.problem_env.env.CheckCollision(self.problem_env.robot, obj)
return col
obj_name_to_pose = {
obj.GetName(): tuple(utils.get_body_xytheta(obj)[0].round(6))
for obj in self.problem_env.objects
}
parent_collides = None if self.parent_state is None else self.parent_state.konf_collisions_with_obj_poses
collisions_at_all_obj_pose_pairs = {}
for obj in self.problem_env.objects:
obj_name_pose_tuple = (obj.GetName(), obj_name_to_pose[obj.GetName()])
collisions_with_obj_did_not_change = parent_collides is not None and obj_name_pose_tuple in parent_collides
if collisions_with_obj_did_not_change:
konfs_in_collision_with_obj = parent_collides[obj_name_pose_tuple]
else:
konfs_in_collision_with_obj = {i for i, q in enumerate(self.key_configs) if
in_collision_with_obj(q, obj)}
collisions_at_all_obj_pose_pairs[obj_name_pose_tuple] = konfs_in_collision_with_obj
return collisions_at_all_obj_pose_pairs
def get_pap_param_with_feasible_motion_plan(self, operator_skeleton,
feasible_op_parameters,
cached_collisions,
cached_holding_collisions):
# getting pick motion - I can still use the cached collisions from state computation
pick_op_params = [op['pick'] for op in feasible_op_parameters]
chosen_pick_param = self.get_op_param_with_feasible_motion_plan(
pick_op_params, cached_collisions)
if not chosen_pick_param['is_feasible']:
return {'is_feasible': False}
target_obj = operator_skeleton.discrete_parameters['object']
if target_obj in self.feasible_pick_params:
self.feasible_pick_params[target_obj].append(chosen_pick_param)
else:
self.feasible_pick_params[target_obj] = [chosen_pick_param]
# self.feasible_pick_params[target_obj].append(pick_op_params)
# getting place motion
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
utils.two_arm_pick_object(
operator_skeleton.discrete_parameters['object'], chosen_pick_param)
place_op_params = [op['place'] for op in feasible_op_parameters]
chosen_place_param = self.get_op_param_with_feasible_motion_plan(
place_op_params, cached_holding_collisions)
utils.two_arm_place_object(chosen_pick_param)
utils.set_robot_config(original_config)
if not chosen_place_param['is_feasible']:
return {'is_feasible': False}
chosen_pap_param = {
'pick': chosen_pick_param,
'place': chosen_place_param,
'is_feasible': True
}
return chosen_pap_param
def __init__(self):
ProblemEnvironment.__init__(self)
problem = MoverProblem(self.env)
self.problem_config = problem.get_problem_config()
self.robot = self.env.GetRobots()[0]
self.objects = self.problem_config['packing_boxes']
self.object_init_poses = {
o.GetName(): get_body_xytheta(o).squeeze()
for o in self.objects
}
self.init_base_conf = np.array([0, 1.05, 0])
self.regions = {
'entire_region': self.problem_config['entire_region'],
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']
}
self.placement_regions = {
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']
}
self.entity_names = [obj.GetName()
for obj in self.objects] + list(self.regions)
self.entity_idx = {
name: idx
for idx, name in enumerate(self.entity_names)
}
self.is_init_pick_node = True
self.name = 'mover'
self.init_saver = CustomStateSaver(self.env)
self.problem_config['env'] = self.env
self.operator_names = ['two_arm_pick', 'two_arm_place']
self.reward_function = None
self.applicable_op_constraint = None
self.two_arm_pick_continuous_constraint = None
self.two_arm_place_continuous_constraint = None
self.objects_to_check_collision = None
def sample_placements(self, pose_ids, collisions, pick_samples, n_smpls):
stttt = time.time()
obj_kinbody = self.abstract_state.problem_env.env.GetKinBody(self.obj)
stime = time.time()
obj_xyth = utils.get_body_xytheta(obj_kinbody)
print 'objxytheta time', time.time() - stime
stime = time.time()
pick_abs_poses = []
for s in pick_samples:
_, poses = utils.get_pick_base_pose_and_grasp_from_pick_parameters(
obj_kinbody, s, obj_xyth)
pick_abs_poses.append(poses)
print "Pick abs pose time", time.time() - stime
stime = time.time()
encoded_pick_abs_poses = np.array([
utils.encode_pose_with_sin_and_cos_angle(s) for s in pick_abs_poses
])
print "Pick pose encoding time", time.time() - stime
pose_ids[:, -6:-2] = encoded_pick_abs_poses
if 'home' in self.region:
chosen_sampler = self.samplers['place_goal_region']
else:
chosen_sampler = self.samplers['place_obj_region']
stime = time.time()
place_samples = chosen_sampler.generate(self.state_vec, pose_ids)
print "prediction time", time.time() - stime
stime = time.time()
place_samples = np.array([
utils.decode_pose_with_sin_and_cos_angle(s) for s in place_samples
])
print "place decoding time", time.time() - stime
# print time.time() - stttt
return place_samples
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:
is_entity_reachable = entity in self.nocollision_pick_op
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 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 __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
paps_used_in_data=None,
use_shortest_path=False):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.parent_state = parent_state
self.goal_entities = goal_entities
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
# cached info
self.use_prm = problem_env.name.find('two_arm') != -1
if self.use_prm:
self.collides, self.current_collides = self.update_collisions_at_prm_vertices(
parent_state)
else:
self.collides = None
self.current_collides = None
# adopt from parent predicate evaluations
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
if parent_state is not None and paps_used_in_data is None:
assert parent_action is not None
moved_obj_type = type(
parent_action.discrete_parameters['two_arm_place_object'])
if moved_obj_type == str or moved_obj_type == unicode:
moved_obj = parent_action.discrete_parameters[
'two_arm_place_object']
else:
moved_obj = parent_action.discrete_parameters[
'two_arm_place_object'].GetName()
self.parent_ternary_predicates = {
(a, b, r): v
for (a, b, r), v in parent_state.ternary_edges.items()
if a != moved_obj and b != moved_obj
}
self.parent_binary_predicates = {
(a, b): v
for (a, b), v in parent_state.binary_edges.items()
if a != moved_obj and b != moved_obj
}
self.use_shortest_path = False
self.cached_place_paths = {}
if paps_used_in_data is None:
self.pick_used = {}
self.place_used = {}
else:
self.pick_used = paps_used_in_data[0]
self.place_used = paps_used_in_data[1]
self.mc_pick_path = {}
self.mc_place_path = {}
self.reachable_entities = []
# predicates
self.pick_in_way = PickInWay(self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=self.use_shortest_path)
self.place_in_way = PlaceInWay(
self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=self.use_shortest_path)
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
self.ternary_edges = self.get_ternary_edges()
self.binary_edges = self.get_binary_edges()
self.nodes = self.get_nodes()
def get_motion_plan(self,
goal,
region_name='entire_region',
n_iterations=None,
cached_collisions=None,
source='',
return_start_set_and_path_idxs=False):
self.problem_env.robot.SetActiveDOFs(
[], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1])
if region_name == 'bridge_region':
region_name = 'entire_region'
region_x = self.problem_env.problem_config[region_name + '_xy'][0]
region_y = self.problem_env.problem_config[region_name + '_xy'][1]
region_x_extents = self.problem_env.problem_config[region_name +
'_extents'][0]
region_y_extents = self.problem_env.problem_config[region_name +
'_extents'][1]
d_fn = base_distance_fn(self.problem_env.robot,
x_extents=region_x_extents,
y_extents=region_y_extents)
s_fn = base_sample_fn(self.problem_env.robot,
x_extents=region_x_extents,
y_extents=region_y_extents,
x=region_x,
y=region_y)
e_fn = base_extend_fn(self.problem_env.robot)
if cached_collisions is not None and self.algorithm == 'prm':
c_fn = set()
for tmp in cached_collisions.values():
c_fn = c_fn.union(tmp)
else:
c_fn = collision_fn(self.problem_env.env, self.problem_env.robot)
q_init = utils.get_body_xytheta(self.problem_env.robot).squeeze()
if n_iterations is None:
n_iterations = [20, 50, 100, 500, 1000]
path = None
status = 'NoSolution'
if self.algorithm == 'rrt':
print "Calling an RRT"
planning_algorithm = rrt_connect
#assert cached_collisions is None
if not isinstance(goal, list):
goal = [goal]
for n_iter in n_iterations:
#print n_iter
for g in goal:
path = planning_algorithm(q_init,
g,
d_fn,
s_fn,
e_fn,
c_fn,
iterations=n_iter)
if path is not None:
return path, 'HasSolution'
else:
print "Calling PRM connect"
planning_algorithm = prm_connect
if return_start_set_and_path_idxs:
path, start_and_prm_idxs = planning_algorithm(
q_init, goal, c_fn, source, return_start_set_and_path_idxs)
else:
path = planning_algorithm(q_init, goal, c_fn, source,
return_start_set_and_path_idxs)
if path is not None:
status = "HasSolution"
if return_start_set_and_path_idxs:
return path, status, start_and_prm_idxs
else:
return path, status
def get_pap_param_with_feasible_motion_plan(self, operator_skeleton,
feasible_op_parameters,
cached_collisions,
cached_holding_collisions):
# getting pick motion - I can still use the cached collisions from state computation
n_feasible = len(feasible_op_parameters)
n_mp_tried = 0
obj_poses = {
o.GetName(): utils.get_body_xytheta(o)
for o in self.problem_env.objects
}
prepick_q0 = utils.get_body_xytheta(self.problem_env.robot)
all_mp_data = []
for op in feasible_op_parameters:
print "n_mp_tried / n_feasible_params = %d / %d" % (n_mp_tried,
n_feasible)
chosen_pick_param = self.get_op_param_with_feasible_motion_plan(
[op['pick']], cached_collisions)
n_mp_tried += 1
mp_data = {
'q0': prepick_q0,
'qg': op['pick']['q_goal'],
'object_poses': obj_poses,
'held_obj': None
}
if not chosen_pick_param['is_feasible']:
print "Pick motion does not exist"
mp_data['label'] = False
all_mp_data.append(mp_data)
continue
else:
mp_data['label'] = True
mp_data['motion'] = chosen_pick_param['motion']
all_mp_data.append(mp_data)
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
utils.two_arm_pick_object(
operator_skeleton.discrete_parameters['object'],
chosen_pick_param)
mp_data = {
'q0': op['pick']['q_goal'],
'qg': op['place']['q_goal'],
'object_poses': obj_poses,
'held_obj': operator_skeleton.discrete_parameters['object'],
'region': operator_skeleton.discrete_parameters['place_region']
}
chosen_place_param = self.get_op_param_with_feasible_motion_plan(
[op['place']], cached_holding_collisions) # calls MP
utils.two_arm_place_object(chosen_pick_param)
utils.set_robot_config(original_config)
if chosen_place_param['is_feasible']:
mp_data['label'] = True
mp_data['motion'] = chosen_place_param['motion']
all_mp_data.append(mp_data)
print 'Motion plan exists'
break
else:
mp_data['label'] = False
all_mp_data.append(mp_data)
print "Place motion does not exist"
pickle.dump(
all_mp_data,
open(
'./planning_experience/motion_planning_experience/' +
str(uuid.uuid4()) + '.pkl', 'wb'))
if not chosen_pick_param['is_feasible']:
print "Motion plan does not exist"
return {'is_feasible': False}
if not chosen_place_param['is_feasible']:
print "Motion plan does not exist"
return {'is_feasible': False}
chosen_pap_param = {
'pick': chosen_pick_param,
'place': chosen_place_param,
'is_feasible': True
}
return chosen_pap_param
def sample_from_discretized_set(self):
(pick_tf, pick_params), (place_tf, place_params) = random.choice(zip(*self.cached_picks))
pick_region = self.problem_env.get_region_containing(self.target_obj)
place_region = self.place_op.discrete_parameters['place_region']
pick_params = copy.deepcopy(pick_params)
place_params = copy.deepcopy(place_params)
old_tf = self.target_obj.GetTransform()
old_pose = get_body_xytheta(self.target_obj).squeeze()
self.pick_op.continuous_parameters = place_params
self.target_obj.SetTransform(place_tf)
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
# This is the only sampling here
place_pose = self.place_generator.sample_from_uniform()
place_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj,
place_pose,
place_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(
self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
if not place_region.contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
place_params['operator_name'] = 'one_arm_place'
place_params['object_pose'] = place_pose
place_params['action_parameters'] = place_pose
place_params['base_pose'] = place_base_pose
place_params['q_goal'][-3:] = place_base_pose
self.place_op.continuous_parameters = place_params
self.pick_op.continuous_parameters = pick_params # is reference and will be changed lader
self.target_obj.SetTransform(pick_tf)
# assert_region(pick_region)
# self.pick_op.execute()
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
pick_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj,
old_pose, pick_region)
pick_params['q_goal'][-3:] = pick_base_pose
# assert_region(pick_region)
# self.target_obj.SetTransform(pick_tf)
# self.pick_op.execute()
# tf = np.dot(np.dot(old_tf, np.linalg.pinv(pick_tf)), self.problem_env.robot.GetTransform())
# self.place_op.execute()
# self.target_obj.SetTransform(old_tf)
# self.problem_env.robot.SetTransform(tf)
# pick_base_pose = get_body_xytheta(self.problem_env.robot).squeeze()
# pick_params['q_goal'][-3:] = pick_base_pose
bad = False
self.target_obj.SetTransform(old_tf)
self.pick_op.execute()
# assert_region(pick_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot):
release_obj()
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
self.place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(
self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
if not self.place_op.discrete_parameters['place_region'].contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
self.target_obj.SetTransform(old_tf)
return pick_params, place_params, 'HasSolution'
