def get_place_feasibility_rate(pick_smpls, place_smpls, target_obj,
problem_env):
# todo start from here
pick_feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker(
problem_env)
parameter_mode = 'ir_params'
place_feasibility_checker = two_arm_place_feasibility_checker.TwoArmPlaceFeasibilityChecker(
problem_env)
n_success = 0
for pick_smpl, place_smpl in zip(pick_smpls, place_smpls):
pick_op = Operator('two_arm_pick', {"object": target_obj})
pick_smpl, status = pick_feasibility_checker.check_feasibility(
pick_op, pick_smpl, parameter_mode=parameter_mode)
pick_op.continuous_parameters = pick_smpl
if status == 'HasSolution':
pick_op.execute()
op = Operator('two_arm_place', {
"object": target_obj,
"place_region": 'loading_region'
},
continuous_parameters=place_smpl)
place_smpl, status = place_feasibility_checker.check_feasibility(
op, place_smpl, parameter_mode='obj_pose')
n_success += status == 'HasSolution'
utils.two_arm_place_object(pick_op.continuous_parameters)
total_samples = len(pick_smpls)
return n_success / float(total_samples) * 100
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 pick_object(self, obj_name):
assert type(obj_name) == str or type(obj_name) == unicode
obj = self.problem_env.env.GetKinBody(obj_name)
# this assumes we have pick
if obj_name in self.pick_used: # where does self.pick_used get updated?
# we cannot use the pick path used in data because q_init is different
motion_plan_goals = self.pick_used[obj_name].continuous_parameters[
'q_goal']
# todo check if pick_used is still valid
# during planning, we would never have pick_used. So we just have to make sure for the data processing
# it is valid if it didn't move
operator = self.pick_used[obj_name]
else:
operator = Operator('two_arm_pick', {'object': obj})
motion_plan_goals = self.generate_potential_pick_configs(
operator, n_pick_configs=10)
if motion_plan_goals is None:
self.sampled_pick_configs_for_objects[obj_name] = None
return None
else:
self.sampled_pick_configs_for_objects[obj_name] = motion_plan_goals
motion_planner = MinimumConstraintPlanner(self.problem_env, obj, 'prm')
motion, status = motion_planner.get_motion_plan(
motion_plan_goals, cached_collisions=self.collides)
# why does it ever enter here?
try:
assert motion is not None
except:
import pdb
pdb.set_trace()
if obj.GetName() not in self.pick_used:
# import pdb;pdb.set_trace()
operator.continuous_parameters = {'q_goal': motion_plan_goals}
self.pick_used[obj.GetName()] = operator
operator.execute()
def determine_collision_and_collision_free_picks(self):
for obj, params in self.pick_params.items():
for pp in params:
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pp
)
before = CustomStateSaver(self.problem_env.env)
pick_op.execute()
if not self.problem_env.env.CheckCollision(self.problem_env.robot):
self.nocollision_pick_op[obj] = pick_op
before.Restore()
break
collisions = {
o for o in self.objects
if self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(o))
}
if obj not in self.collision_pick_op or len(collisions) < len(self.collision_pick_op[obj][1]):
self.collision_pick_op[obj] = pick_op, collisions
before.Restore()
def count_collides(papp):
before = CustomStateSaver(self.problem_env.env)
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
pick_op.execute()
pick_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
place_op.execute()
place_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
before.Restore()
return pick_collisions + place_collisions
def fcn(o, r, s):
global num_ik_checks
global num_mp_checks
if problem.name == 'two_arm_mover':
abstract_state = None # TODO: figure out what goes here
abstract_action = Operator(
'two_arm_pick_two_arm_place', {
'object': problem.env.GetKinBody(o),
'place_region': problem.regions[r]
})
sampler = UniformSampler(problem.regions[r])
generator = TwoArmPaPGenerator(
abstract_state,
abstract_action,
sampler,
n_parameters_to_try_motion_planning=config.n_mp_limit,
n_iter_limit=config.n_iter_limit,
problem_env=problem,
pick_action_mode='ir_parameters',
place_action_mode='object_pose')
while True:
s.Restore()
prev_ik_checks = generator.n_ik_checks
prev_mp_checks = generator.n_mp_checks
params = generator.sample_next_point()
num_ik_checks += generator.n_ik_checks - prev_ik_checks
num_mp_checks += generator.n_mp_checks - prev_mp_checks
if params['is_feasible']:
abstract_action.continuous_parameters = params
abstract_action.execute()
t = CustomStateSaver(problem.env)
yield params, t
else:
yield None
elif problem.name == 'one_arm_mover':
while True:
s.Restore()
action = Operator(
'one_arm_pick_one_arm_place', {
'object': problem.env.GetKinBody(o),
'place_region': problem.regions[r]
})
current_region = problem.get_region_containing(
problem.env.GetKinBody(o)).name
sampler = OneArmPaPUniformGenerator(
action,
problem,
cached_picks=(iksolutions[current_region], iksolutions[r]))
pick_params, place_params, status = sampler.sample_next_point(
max_ik_attempts=500)
if status == 'HasSolution':
action.continuous_parameters = {
'pick': pick_params,
'place': place_params
}
action.execute()
t = CustomStateSaver(problem.env)
yield action.continuous_parameters, t
else:
yield None
else:
raise NotImplementedError
def initialize_pap_pick_place_params(self, moved_obj, parent_state):
self.problem_env.disable_objects()
stime = time.time()
# sorted_objects = sorted(self.objects, key=lambda o: o not in self.goal_entities)
# sorted_regions = sorted(self.regions, key=lambda r: r not in self.goal_entities)
assert len(self.goal_entities) == 2
assert 'c_obst1' in self.goal_entities
goal_obj = self.problem_env.goal_objects[0]
goal_region = self.problem_env.goal_region
sorted_objects = [goal_obj] + [o for o in self.objects if o not in self.goal_entities]
sorted_regions = [goal_region] + [r for r in self.regions if r not in self.goal_entities]
all_goals_are_reachable = True
for obj in self.objects:
self.pick_params[obj] = []
for r in self.regions:
self.pap_params[(obj, r)] = []
self.place_params[(obj, r)] = []
num_iks = 0
# used to gauge how many iks we did, so that we can put sleep in our state computation
# num ik attempts is 5 for non-goal objects and 20 for goal objects. Let's say we do 10 on average?
for obj in sorted_objects:
if all_goals_are_reachable and obj not in self.goal_entities:
break
obj_object = self.problem_env.env.GetKinBody(obj)
old_tf = obj_object.GetTransform()
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj_object})
for r in sorted_regions:
place_op = Operator(operator_type='one_arm_place',
discrete_parameters={'object': obj_object, 'region': self.problem_env.regions[r]})
# What are these values?
if obj in self.goal_entities and r in self.goal_entities:
num_tries = 20
num_iters = 50
elif obj not in self.goal_entities and r in self.goal_entities:
num_iters = 0
else:
num_tries = 5
num_iters = 10
# Re-use the pap parameters
if self.parent_state is not None and obj != moved_obj:
self.pap_params[(obj, r)] = parent_state.pap_params[(obj, r)]
else:
self.pap_params[(obj, r)] = []
# check existing solutions. If we have no collision solution, then no need to sample new values
if (obj, r) in self.pap_params:
nocollision = False
self.problem_env.enable_objects()
for pick_params, place_params in self.pap_params[(obj, r)]:
# checking pick has no collision
collision = False
pick_op.continuous_parameters = pick_params
pick_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
####
# checking place has no collision
place_op.continuous_parameters = place_params
place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
if self.problem_env.env.CheckCollision(obj_object):
collision = True
obj_object.SetTransform(old_tf)
if not collision:
nocollision = True
break
####
self.problem_env.disable_objects()
if nocollision:
# we already have a nocollision solution, move onto the next region
self.pick_params[obj].append(pick_params)
self.place_params[(obj, r)].append(place_params)
print('state computation: already have nocollision', obj, r)
continue
### end of checking existing pick and place samples
# No collision-free pick and place exists. Sampling new paps
nocollision = self.sample_new_parameters(pick_op, place_op, old_tf, num_iters, num_tries, obj, r)
if not nocollision and obj in self.goal_entities and r in self.goal_entities:
all_goals_are_reachable = False
if obj in self.goal_entities and len(self.pick_params[obj]) == 0:
# We must have at least one pap for goal entitiy
# Previously, self.pick_params could have been empty
# what happens if self.pick_params is empty?
# It computes the predicates incorrectly. More concretely,
# The object will not be reachable, because self.nocollision_pick will never be computed
# InWay(obj_k, target_obj) will never be computed, because obj is not in self.collision_pick is empty
# And similarly for the ternary predicate.
if self.parent_state is None:
stime = time.time()
while len(self.pick_params[obj]) == 0:
nocollision = self.sample_new_parameters(pick_op, place_op, old_tf, num_iters, num_tries, obj, r)
if not nocollision and obj in self.goal_entities and r in self.goal_entities:
all_goals_are_reachable = False
if time.time()-stime > 1000:
break
else:
self.pick_params[obj] = self.parent_state.pick_params[obj]
# if obj in self.goal_entities and r in self.goal_entities:
# print self.pap_params[(obj, r)]
# print time.time()-stime
self.problem_env.enable_objects()
def determine_collision_and_collision_free_places(self):
before = CustomStateSaver(self.problem_env.env)
for obj in self.objects:
for r in self.regions:
# print obj
if len(self.pap_params[(obj, r)]) > 0:
def count_collides(papp):
before = CustomStateSaver(self.problem_env.env)
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
pick_op.execute()
pick_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
place_op.execute()
place_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
before.Restore()
return pick_collisions + place_collisions
# chooses the one with minimal collisions
papp = min(self.pap_params[(obj, r)], key=lambda papp: count_collides(papp))
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
pick_op.execute()
# pap gets placed in nocollision if both pick and place have no collision
# otherwise it gets placed in collision, along with objects that collide with the place config (but not the pick config)
pick_collision = self.problem_env.env.CheckCollision(self.problem_env.robot)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
place_op.execute()
place_collision = self.problem_env.env.CheckCollision(self.problem_env.robot) \
or self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(obj))
if not pick_collision and not place_collision:
self.nocollision_place_op[(obj, r)] = pick_op, place_op
# if obj in self.goal_entities and r in self.goal_entities:
# print('successful goal pap')
before.Restore()
continue
collisions = {
o for o in self.objects
if self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(o))
}
if (obj, r) not in self.collision_place_op or len(collisions) < \
len(self.collision_place_op[(obj, r)][1]):
self.collision_place_op[(obj, r)] = place_op, collisions
before.Restore()
class OneArmPaPUniformGenerator:
def __init__(self, operator_skeleton, problem_env, swept_volume_constraint=None, cached_picks=None):
self.problem_env = problem_env
self.cached_picks = cached_picks
target_region = None
if 'place_region' in operator_skeleton.discrete_parameters:
target_region = operator_skeleton.discrete_parameters['place_region']
if type(target_region) == str:
target_region = self.problem_env.regions[target_region]
target_obj = operator_skeleton.discrete_parameters['object']
self.robot = problem_env.robot
self.target_region = target_region
self.target_obj = target_obj
self.swept_volume_constraint = swept_volume_constraint
self.pick_op = Operator(operator_type='one_arm_pick',
discrete_parameters={'object': target_obj})
self.pick_generator = UniformGenerator(self.pick_op, problem_env) # just use generator, I think
self.place_op = Operator(operator_type='one_arm_place',
discrete_parameters={'object': target_obj, 'place_region': target_region},
continuous_parameters={})
self.place_generator = UniformGenerator(self.place_op, problem_env)
self.pick_feasibility_checker = OneArmPickFeasibilityChecker(problem_env)
self.place_feasibility_checker = OneArmPlaceFeasibilityChecker(problem_env)
self.operator_skeleton = operator_skeleton
self.n_ik_checks = 0
self.n_mp_checks = 0
def sample_next_point(self, max_ik_attempts):
# n_iter refers to the max number of IK attempts on pick
n_ik_attempts = 0
while True:
pick_cont_params, place_cont_params, status = self.sample_cont_params()
self.n_ik_checks += 1
n_ik_attempts += 1
if status == 'InfeasibleIK' or status == 'InfeasibleBase':
if n_ik_attempts == max_ik_attempts:
break
elif status == 'HasSolution':
return pick_cont_params, place_cont_params, 'HasSolution'
return None, None, 'NoSolution'
def is_base_feasible(self, base_pose):
utils.set_robot_config(base_pose, self.robot)
robot_aabb = self.robot.ComputeAABB()
inside_region = self.problem_env.regions['home_region'].contains(robot_aabb)
# or self.problem_env.regions['loading_region'].contains(robot_aabb)
no_collision = not self.problem_env.env.CheckCollision(self.robot)
if (not inside_region) or (not no_collision):
return False
else:
return True
def sample_pick_cont_parameters(self):
op_parameters = self.pick_generator.sample_from_uniform()
grasp_params, pick_base_pose = get_pick_base_pose_and_grasp_from_pick_parameters(self.target_obj, op_parameters)
if not self.is_base_feasible(pick_base_pose):
return None, 'InfeasibleBase'
utils.open_gripper()
grasps = grasp_utils.compute_one_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=self.target_obj,
robot=self.robot)
grasp_config, grasp = grasp_utils.solveIKs(self.problem_env.env, self.robot, grasps)
param = {'q_goal': np.hstack([grasp_config, pick_base_pose]),
'grasp_params': grasp_params,
'g_config': grasp_config,
'action_parameters': op_parameters}
if grasp_config is None:
return None, 'InfeasibleIK'
else:
return param, 'HasSolution'
def sample_place_cont_parameters(self, pick_params):
obj_place_pose = self.place_generator.sample_from_uniform()
self.place_op.continuous_parameters['grasp_params'] = pick_params['grasp_params']
cont_params, status = self.place_feasibility_checker.check_feasibility(self.place_op, obj_place_pose,
self.swept_volume_constraint)
if status != 'HasSolution':
return None, status
else:
return cont_params, status
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'
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 sample_cont_params(self):
assert len(self.robot.GetGrabbed()) == 0
if self.cached_picks is not None:
return self.sample_from_discretized_set()
else:
# sampling:
# - pick parameters: ir and grasp parameters
# - place parameters: object place pose; uses the same grasp parameters as pick
return self.sample_from_continuous_space()
def initialize_pap_pick_place_params(self, moved_obj, parent_state):
self.problem_env.disable_objects()
stime=time.time()
sorted_objects = sorted(self.objects, key=lambda o: o not in self.goal_entities)
sorted_regions = sorted(self.regions, key=lambda r: r not in self.goal_entities)
all_goals_are_reachable = True
for obj in self.objects:
self.pick_params[obj] = []
for r in self.regions:
self.pap_params[(obj, r)] = []
self.place_params[(obj, r)] = []
num_iks = 0
# used to gauge how many iks we did, so that we can put sleep in our state computation
# num ik attempts is 5 for non-goal objects and 20 for goal objects. Let's say we do 10 on average?
for obj in sorted_objects:
if all_goals_are_reachable and obj not in self.goal_entities:
break
obj_object = self.problem_env.env.GetKinBody(obj)
old_tf = obj_object.GetTransform()
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj_object})
for r in sorted_regions:
#print(obj, r)
place_op = Operator(operator_type='one_arm_place', discrete_parameters={'object': obj_object, 'region': self.problem_env.regions[r]})
current_region = self.problem_env.get_region_containing(obj).name
if obj in self.goal_entities and r in self.goal_entities:
num_tries = 20
num_iters = 50
elif obj not in self.goal_entities and r in self.goal_entities:
num_iters = 0
else:
num_tries = 5
num_iters = 10
if self.parent_state is not None and obj != moved_obj:
self.pap_params[(obj, r)] = parent_state.pap_params[(obj, r)]
else:
self.pap_params[(obj, r)] = []
op_skel = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]})
# It easily samples without cached iks?
papg = OneArmPaPUniformGenerator(op_skel, self.problem_env,
cached_picks=(self.iksolutions[current_region], self.iksolutions[r]))
# check existing solutions
if (obj, r) in self.pap_params:
nocollision = False
self.problem_env.enable_objects()
for pick_params, place_params in self.pap_params[(obj, r)]:
collision = False
pick_op.continuous_parameters = pick_params
pick_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
place_op.continuous_parameters = place_params
place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
if self.problem_env.env.CheckCollision(obj_object):
collision = True
obj_object.SetTransform(old_tf)
if not collision:
nocollision = True
break
self.problem_env.disable_objects()
if nocollision:
# we already have a nocollision solution
#print('already have nocollision', obj, r)
continue
# I think num_iters is the number of paps for each object
nocollision = False
for _ in range(num_iters - len(self.pap_params[(obj, r)])):
pick_params, place_params, status = papg.sample_next_point(num_tries)
if 'HasSolution' in status:
self.pap_params[(obj, r)].append((pick_params, place_params))
self.pick_params[obj].append(pick_params)
#print('success')
self.problem_env.enable_objects()
collision = False
pick_op.continuous_parameters = pick_params
pick_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
place_op.continuous_parameters = place_params
place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
collision = True
if self.problem_env.env.CheckCollision(obj_object):
collision = True
obj_object.SetTransform(old_tf)
self.problem_env.disable_objects()
if not collision:
nocollision = True
#print('found nocollision', obj, r)
break
if not nocollision and obj in self.goal_entities and r in self.goal_entities:
all_goals_are_reachable = False
#if obj in self.goal_entities and r in self.goal_entities:
# print self.pap_params[(obj, r)]
#print time.time()-stime
self.problem_env.enable_objects()
def search(mover, config, pap_model, goal_objs, goal_region_name,
learned_sampler_model):
tt = time.time()
print "Greedy search began"
goal_region = mover.placement_regions[goal_region_name]
obj_names = [obj.GetName() for obj in mover.objects]
n_objs_pack = config.n_objs_pack
statecls = get_state_class(config.domain)
goal = mover.goal_entities
mover.reset_to_init_state_stripstream()
depth_limit = 60
# lowest valued items are retrieved first in PriorityQueue
search_queue = Queue.PriorityQueue(
) # (heuristic, nan, operator skeleton, state. trajectory);a
print "State computation..."
#if os.path.isfile('tmp.pkl'):
# state = pickle.load(open('tmp.pkl', 'r'))
#else:
state = statecls(mover, goal)
#state.make_pklable()
#pickle.dump(state, open('tmp.pkl', 'wb'))
state.make_plannable(mover)
initnode = Node(None, None, state)
actions = get_actions(mover, goal, config)
nodes = [initnode]
update_search_queue(state, actions, initnode, search_queue, pap_model,
mover, config)
iter = 0
# beginning of the planner
print "Beginning of the while-loop"
while True:
iter += 1
curr_time = time.time() - tt
print "Time %.2f / %.2f " % (curr_time, config.timelimit)
print "Iter %d / %d" % (iter, config.num_node_limit)
if curr_time > config.timelimit or iter > config.num_node_limit:
return None, None, iter, nodes
# persistency
if search_queue.empty():
actions = get_actions(mover, goal, config)
for a in actions:
discrete_params = (a.discrete_parameters['object'],
a.discrete_parameters['place_region'])
hval = initnode.heuristic_vals[discrete_params]
search_queue.put(
(hval, float('nan'), a, initnode)) # initial q
# can a node be associated with different action? I think so.
# For example, the init node can be associated with many actions
curr_hval, _, action, node = search_queue.get()
print "Chosen abstract action", action.discrete_parameters[
'object'], action.discrete_parameters['place_region']
state = node.state
print "Curr hval", curr_hval
if node.depth > depth_limit:
print('skipping because depth limit',
node.action.discrete_parameters.values())
# reset to state
state.restore(mover)
if action.type == 'two_arm_pick_two_arm_place':
print("Sampling for {}".format(
action.discrete_parameters.values()))
smpled_param = sample_continuous_parameters(
state, action, node, learned_sampler_model, config)
if smpled_param['is_feasible']:
action.continuous_parameters = smpled_param
action.execute()
print "Action executed"
else:
print "Failed to sample an action"
continue
is_goal_achieved = np.all([
goal_region.contains(mover.env.GetKinBody(o).ComputeAABB())
for o in goal_objs
])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
node.is_goal_traj = True
nodes_to_goal = list(node.backtrack(
))[::-1] # plan of length 0 is possible I think
if config.gather_planning_exp:
newstate = statecls(mover, goal, node.state, action)
newnode = Node(node, action, newstate)
newnode.is_goal_traj = True
nodes_to_goal.append(newnode)
nodes.append(newnode)
plan = [nd.parent_action for nd in nodes_to_goal[1:]]
else:
plan = [nd.parent_action
for nd in nodes_to_goal[1:]] + [action]
for plan_action, nd in zip(plan, nodes_to_goal):
nd.is_goal_traj = True
nd.executed_action = plan_action
return nodes_to_goal, plan, iter, nodes
else:
stime = time.time()
newstate = statecls(mover, goal, node.state, action)
print "New state computation time ", time.time() - stime
newnode = Node(node, action, newstate)
newactions = get_actions(mover, goal, config)
update_search_queue(newstate, newactions, newnode,
search_queue, pap_model, mover, config)
nodes.append(newnode)
elif action.type == 'one_arm_pick_one_arm_place':
print("Sampling for {}".format(
action.discrete_parameters.values()))
success = False
o = action.discrete_parameters['object']
r = action.discrete_parameters['place_region']
if (o, r) in state.nocollision_place_op:
print "Already no collision place op"
pick_op, place_op = node.state.nocollision_place_op[(o, r)]
pap_params = pick_op.continuous_parameters, place_op.continuous_parameters
else:
mover.enable_objects()
# papg = OneArmPaPUniformGenerator(action, mover,
# cached_picks=None)
# pick_params, place_params, status = papg.sample_next_point(200)
stime = time.time()
pick_params, place_params, status = sample_continuous_parameters(
state, action, node, learned_sampler_model, config)
print "Sampling time", time.time() - stime
if status == 'HasSolution':
pap_params = pick_params, place_params
else:
pap_params = None
if pap_params is not None:
pick_params, place_params = pap_params
action = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={
'object': o,
'region': mover.regions[r],
},
continuous_parameters={
'pick': pick_params,
'place': place_params,
})
action.execute()
success = True
is_goal_achieved = \
np.all([mover.regions['rectangular_packing_box1_region'].contains(
mover.env.GetKinBody(o).ComputeAABB()) for o in obj_names[:n_objs_pack]])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
node.is_goal_traj = True
nodes_to_goal = list(node.backtrack(
))[::-1] # plan of length 0 is possible I think
plan = [nd.parent_action
for nd in nodes_to_goal[1:]] + [action]
return nodes_to_goal, plan, iter, nodes
else:
newstate = statecls(mover, goal, node.state, action)
newnode = Node(node, action, newstate)
newnode.generators[(
o, r
)] = papg # TODO: count iks for final action (which has no node)
nodes.append(newnode)
newactions = get_actions(mover, goal, config)
update_search_queue(newstate, newactions, newnode,
search_queue, pap_model, mover, config)
if not success:
print('failed to execute action')
else:
print('action successful')
else:
raise NotImplementedError
def main():
problem_idx = 0
env_name = 'two_arm_mover'
if env_name == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(problem_idx)
goal = ['rectangular_packing_box1_region'
] + [obj.GetName() for obj in problem_env.objects[:3]]
state_fname = 'one_arm_state_gpredicate.pkl'
else:
problem_env = PaPMoverEnv(problem_idx)
goal_objs = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4'
]
goal_region = 'home_region'
goal = [goal_region] + goal_objs
state_fname = 'two_arm_state_gpredicate.pkl'
problem_env.set_goal(goal)
if os.path.isfile(state_fname):
state = pickle.load(open(state_fname, 'r'))
else:
statecls = helper.get_state_class(env_name)
state = statecls(problem_env, problem_env.goal)
utils.viewer()
if env_name == 'one_arm_mover':
obj_name = 'c_obst9'
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={
'object': problem_env.env.GetKinBody(obj_name)
},
continuous_parameters=state.pick_params[obj_name][0])
pick_op.execute()
problem_env.env.Remove(problem_env.env.GetKinBody('computer_table'))
utils.set_color(obj_name, [0.9, 0.8, 0.0])
utils.set_color('c_obst0', [0, 0, 0.8])
utils.set_obj_xytheta(
np.array([[4.47789478, -0.01477591, 4.76236795]]), 'c_obst0')
T_viewer = np.array(
[[-0.69618481, -0.41674492, 0.58450867, 3.62774134],
[-0.71319601, 0.30884202, -0.62925993, 0.39102399],
[0.08172004, -0.85495045, -0.51223194, 1.70261502],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
utils.release_obj()
else:
T_viewer = np.array(
[[0.99964468, -0.00577897, 0.02602139, 1.66357124],
[-0.01521307, -0.92529857, 0.37893419, -7.65383244],
[0.02188771, -0.37919541, -0.92505771, 6.7393589],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
# prefree and occludespre
target = 'rectangular_packing_box4'
utils.set_obj_xytheta(np.array([[0.1098148, -6.33305931, 0.22135689]]),
target)
utils.get_body_xytheta(target)
utils.visualize_path(
state.cached_pick_paths['rectangular_packing_box4'])
import pdb
pdb.set_trace()
# manipfree and occludesmanip
pick_obj = 'square_packing_box2'
pick_used = state.pick_used[pick_obj]
utils.two_arm_pick_object(pick_obj, pick_used.continuous_parameters)
utils.visualize_path(state.cached_place_paths[(u'square_packing_box2',
'home_region')])
import pdb
pdb.set_trace()
class OneArmPaPGenerator:
def __init__(self, operator_skeleton, n_iter_limit, problem_env, pick_sampler=None, place_sampler=None):
self.problem_env = problem_env
self.n_iter_limit = n_iter_limit
target_region = None
if 'place_region' in operator_skeleton.discrete_parameters:
target_region = operator_skeleton.discrete_parameters['place_region']
if type(target_region) is str or type(target_region) is unicode:
target_region = self.problem_env.regions[target_region]
target_obj = operator_skeleton.discrete_parameters['object']
self.robot = problem_env.robot
self.target_region = target_region
self.target_obj = target_obj
if type(self.target_obj) is str or type(self.target_obj) is unicode:
self.target_obj = self.problem_env.env.GetKinBody(self.target_obj)
# todo change this to use the sampler passed in
self.pick_op = Operator(operator_type='one_arm_pick',
discrete_parameters={'object': target_obj})
self.pick_sampler = pick_sampler
self.place_op = Operator(operator_type='one_arm_place',
discrete_parameters={'object': target_obj, 'place_region': target_region},
continuous_parameters={})
self.place_sampler = place_sampler
self.pick_feasibility_checker = OneArmPickFeasibilityChecker(problem_env)
self.robot_base_place_feasibility_checker = OneArmPlaceFeasibilityChecker(problem_env,
action_mode='robot_base_pose')
self.obj_base_place_feasibility_checker = OneArmPlaceFeasibilityChecker(problem_env,
action_mode='object_pose')
self.operator_skeleton = operator_skeleton
self.n_pick_mp_checks = 0
self.n_mp_checks = 0
self.n_mp_infeasible = 0
self.n_place_mp_checks = 0
self.n_pick_mp_infeasible = 0
self.n_place_mp_infeasible = 0
self.n_ik_checks = 0
def sample_next_point(self, samples_tried=None, sample_values=None):
# n_iter refers to the max number of IK attempts on pick
n_ik_attempts = 0
n_base = 0
stime = time.time()
#while True:
for i in range(2000):
pick_cont_params, place_cont_params, status = self.sample_from_continuous_space()
if status == 'InfeasibleIK':
n_ik_attempts += 1
self.n_ik_checks += 1
if n_ik_attempts == self.n_iter_limit:
break
elif status == 'HasSolution':
return pick_cont_params, place_cont_params, 'HasSolution'
else:
n_base += 1
print time.time()-stime
return None, None, 'NoSolution'
def is_base_feasible(self, base_pose):
utils.set_robot_config(base_pose, self.robot)
robot_aabb = self.robot.ComputeAABB()
inside_region = self.problem_env.regions['home_region'].contains(robot_aabb)
# or self.problem_env.regions['loading_region'].contains(robot_aabb)
no_collision = not self.problem_env.env.CheckCollision(self.robot)
if (not inside_region) or (not no_collision):
return False
else:
return True
def sample_pick_cont_parameters(self):
op_parameters = self.pick_sampler.sample()
grasp_params, pick_base_pose = get_pick_base_pose_and_grasp_from_pick_parameters(self.target_obj, op_parameters)
if not self.is_base_feasible(pick_base_pose):
return None, 'InfeasibleBase'
utils.open_gripper()
grasps = grasp_utils.compute_one_arm_grasp(depth_portion=grasp_params[2],
height_portion=grasp_params[1],
theta=grasp_params[0],
obj=self.target_obj,
robot=self.robot)
grasp_config, grasp = grasp_utils.solveIKs(self.problem_env.env, self.robot, grasps)
param = {'q_goal': np.hstack([grasp_config, pick_base_pose]),
'grasp_params': grasp_params,
'g_config': grasp_config,
'action_parameters': op_parameters}
if grasp_config is None:
return None, 'InfeasibleIK'
else:
return param, 'HasSolution'
def sample_place_cont_parameters(self, pick_params):
obj_place_pose = self.place_sampler.sample()
self.place_op.continuous_parameters['grasp_params'] = pick_params['grasp_params']
# We only have the below for the learned sampler
is_using_learning = self.place_sampler.samplers is not None
if is_using_learning:
if 'rectangular_packing_box1_region' in self.target_region.name:
sampler_to_use = self.place_sampler.samplers['place_goal_region']
elif 'center_shelf_region' in self.target_region.name:
sampler_to_use = self.place_sampler.samplers['place_obj_region']
else:
raise NotImplementedError
# We dont learn sampler for goal region
if 'Uniform' in sampler_to_use.__class__.__name__:
cont_params, status = self.obj_base_place_feasibility_checker.check_feasibility(self.place_op,
obj_place_pose)
elif 'WGAN' in sampler_to_use.__class__.__name__:
cont_params, status = self.robot_base_place_feasibility_checker.check_feasibility(self.place_op,
obj_place_pose)
else:
raise NotImplementedError
else:
cont_params, status = self.obj_base_place_feasibility_checker.check_feasibility(self.place_op,
obj_place_pose)
if status != 'HasSolution':
return None, status
else:
return cont_params, status
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 get_problem(mover):
tt = time.time()
obj_names = [obj.GetName() for obj in mover.objects]
n_objs_pack = config.n_objs_pack
if config.domain == 'two_arm_mover':
statecls = ShortestPathPaPState
goal = ['home_region'] + [obj.GetName() for obj in mover.objects[:n_objs_pack]]
elif config.domain == 'one_arm_mover':
def create_one_arm_pap_state(*args, **kwargs):
while True:
state = OneArmPaPState(*args, **kwargs)
if len(state.nocollision_place_op) > 0:
return state
else:
print('failed to find any paps, trying again')
statecls = create_one_arm_pap_state
goal = ['rectangular_packing_box1_region'] + [obj.GetName() for obj in mover.objects[:n_objs_pack]]
else:
raise NotImplementedError
if config.visualize_plan:
mover.env.SetViewer('qtcoin')
set_viewer_options(mover.env)
pr = cProfile.Profile()
pr.enable()
state = statecls(mover, goal)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(30)
pstats.Stats(pr).sort_stats('cumtime').print_stats(30)
#state.make_pklable()
mconfig_type = collections.namedtuple('mconfig_type',
'operator n_msg_passing n_layers num_fc_layers n_hidden no_goal_nodes top_k optimizer lr use_mse batch_size seed num_train val_portion num_test mse_weight diff_weight_msg_passing same_vertex_model weight_initializer loss use_region_agnostic')
assert config.num_train <= 5000
pap_mconfig = mconfig_type(
operator='two_arm_pick_two_arm_place',
n_msg_passing=1,
n_layers=2,
num_fc_layers=2,
n_hidden=32,
no_goal_nodes=False,
top_k=1,
optimizer='adam',
lr=1e-4,
use_mse=True,
batch_size='32',
seed=config.train_seed,
num_train=config.num_train,
val_portion=.1,
num_test=1882,
mse_weight=1.0,
diff_weight_msg_passing=False,
same_vertex_model=False,
weight_initializer='glorot_uniform',
loss=config.loss,
use_region_agnostic=False
)
if config.domain == 'two_arm_mover':
num_entities = 11
n_regions = 2
elif config.domain == 'one_arm_mover':
num_entities = 12
n_regions = 2
else:
raise NotImplementedError
num_node_features = 10
num_edge_features = 44
entity_names = mover.entity_names
with tf.variable_scope('pap'):
pap_model = PaPGNN(num_entities, num_node_features, num_edge_features, pap_mconfig, entity_names, n_regions)
pap_model.load_weights()
mover.reset_to_init_state_stripstream()
depth_limit = 60
class Node(object):
def __init__(self, parent, action, state, reward=0):
self.parent = parent # parent.state is initial state
self.action = action
self.state = state # resulting state
self.reward = reward # resulting reward
if parent is None:
self.depth = 1
else:
self.depth = parent.depth + 1
def backtrack(self):
node = self
while node is not None:
yield node
node = node.parent
action_queue = Queue.PriorityQueue() # (heuristic, nan, operator skeleton, state. trajectory)
initnode = Node(None, None, state)
initial_state = state
actions = get_actions(mover, goal, config)
for a in actions:
hval = compute_heuristic(state, a, pap_model, mover)
action_queue.put((hval, float('nan'), a, initnode)) # initial q
iter = 0
# beginning of the planner
while True:
if time.time() - tt > config.timelimit:
return None, iter
iter += 1
#if 'one_arm' in mover.name:
# time.sleep(3.5) # gauged using max_ik_attempts = 20
if iter > 3000:
print('failed to find plan: iteration limit')
return None, iter
if action_queue.empty():
actions = get_actions(mover, goal, config)
for a in actions:
action_queue.put((compute_heuristic(initial_state, a, pap_model, mover), float('nan'), a, initnode)) # initial q
#import pdb;pdb.set_trace()
curr_hval, _, action, node = action_queue.get()
state = node.state
print('\n'.join([str(parent.action.discrete_parameters.values()) for parent in list(node.backtrack())[-2::-1]]))
print("{}".format(action.discrete_parameters.values()))
if node.depth >= 2 and action.type == 'two_arm_pick' and node.parent.action.discrete_parameters['object'] == \
action.discrete_parameters['object']: # and plan[-1][1] == r:
print('skipping because repeat', action.discrete_parameters['object'])
continue
if node.depth > depth_limit:
print('skipping because depth limit', node.action.discrete_parameters.values())
# reset to state
state.restore(mover)
#utils.set_color(action.discrete_parameters['object'], [1, 0, 0]) # visualization purpose
if action.type == 'two_arm_pick_two_arm_place':
smpler = PaPUniformGenerator(action, mover, None)
smpled_param = smpler.sample_next_point(action, n_iter=200, n_parameters_to_try_motion_planning=3,
cached_collisions=state.collides, cached_holding_collisions=None)
if smpled_param['is_feasible']:
action.continuous_parameters = smpled_param
action.execute()
print "Action executed"
else:
print "Failed to sample an action"
#utils.set_color(action.discrete_parameters['object'], [0, 1, 0]) # visualization purpose
continue
is_goal_achieved = \
np.all([mover.regions['home_region'].contains(mover.env.GetKinBody(o).ComputeAABB()) for o in
obj_names[:n_objs_pack]])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
print iter, time.time()-tt
import pdb;pdb.set_trace()
"""
trajectory = Trajectory(mover.seed, mover.seed)
plan = list(node.backtrack())[::-1] # plan of length 0 is possible I think
trajectory.states = [nd.state for nd in plan]
trajectory.actions = [nd.action for nd in plan[1:]] + [action]
trajectory.rewards = [nd.reward for nd in plan[1:]] + [0]
trajectory.state_prime = [nd.state for nd in plan[1:]]
trajectory.seed = mover.seed
print(trajectory)
return trajectory, iter
"""
else:
newstate = statecls(mover, goal, node.state, action)
print "New state computed"
newstate.make_pklable()
newnode = Node(node, action, newstate)
newactions = get_actions(mover, goal, config)
print "Old h value", curr_hval
for newaction in newactions:
hval = compute_heuristic(newstate, newaction, pap_model, mover) - 1. * newnode.depth
print "New state h value %.4f for %s %s" % (
hval, newaction.discrete_parameters['object'], newaction.discrete_parameters['region'])
action_queue.put(
(hval, float('nan'), newaction, newnode))
import pdb;pdb.set_trace()
#utils.set_color(action.discrete_parameters['object'], [0, 1, 0]) # visualization purpose
elif action.type == 'one_arm_pick_one_arm_place':
success = False
obj = action.discrete_parameters['object']
region = action.discrete_parameters['region']
o = obj.GetName()
r = region.name
if (o, r) in state.nocollision_place_op:
pick_op, place_op = node.state.nocollision_place_op[(o, r)]
pap_params = pick_op.continuous_parameters, place_op.continuous_parameters
else:
mover.enable_objects()
current_region = mover.get_region_containing(obj).name
papg = OneArmPaPUniformGenerator(action, mover, cached_picks=(node.state.iksolutions[current_region], node.state.iksolutions[r]))
pick_params, place_params, status = papg.sample_next_point(500)
if status == 'HasSolution':
pap_params = pick_params, place_params
else:
pap_params = None
if pap_params is not None:
pick_params, place_params = pap_params
action = Operator(
operator_type='one_arm_pick_one_arm_place',
discrete_parameters={
'object': obj,
'region': mover.regions[r],
},
continuous_parameters={
'pick': pick_params,
'place': place_params,
}
)
action.execute()
success = True
is_goal_achieved = \
np.all([mover.regions['rectangular_packing_box1_region'].contains(
mover.env.GetKinBody(o).ComputeAABB()) for o in obj_names[:n_objs_pack]])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
trajectory = Trajectory(mover.seed, mover.seed)
plan = list(node.backtrack())[::-1]
trajectory.states = [nd.state for nd in plan]
for s in trajectory.states:
s.pap_params = None
s.pick_params = None
s.place_params = None
s.nocollision_pick_op = None
s.collision_pick_op = None
s.nocollision_place_op = None
s.collision_place_op = None
trajectory.actions = [nd.action for nd in plan[1:]] + [action]
for op in trajectory.actions:
op.discrete_parameters = {
key: value.name if 'region' in key else value.GetName()
for key, value in op.discrete_parameters.items()
}
trajectory.rewards = [nd.reward for nd in plan[1:]]
trajectory.state_prime = [nd.state for nd in plan[1:]]
trajectory.seed = mover.seed
print(trajectory)
return trajectory, iter
else:
pr = cProfile.Profile()
pr.enable()
newstate = statecls(mover, goal, node.state, action)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(30)
pstats.Stats(pr).sort_stats('cumtime').print_stats(30)
print "New state computed"
newnode = Node(node, action, newstate)
newactions = get_actions(mover, goal, config)
print "Old h value", curr_hval
for newaction in newactions:
hval = compute_heuristic(newstate, newaction, pap_model, mover) - 1. * newnode.depth
action_queue.put((hval, float('nan'), newaction, newnode))
if not success:
print('failed to execute action')
else:
print('action successful')
else:
raise NotImplementedError
def search(mover, config, pap_model, learned_smpler=None):
tt = time.time()
obj_names = [obj.GetName() for obj in mover.objects]
n_objs_pack = config.n_objs_pack
statecls = get_state_class(config.domain)
goal = mover.goal
mover.reset_to_init_state_stripstream()
depth_limit = 60
state = statecls(mover, goal)
# utils.viewer()
"""
actions = get_actions(mover, goal, config)
action = actions[0]
utils.set_color(action.discrete_parameters['object'], [1, 0, 0])
state_vec = create_state_vec(state.key_config_obstacles, action, goal)
smpler = LearnedGenerator(action, mover, learned_smpler, state_vec)
smples = np.vstack([smpler.sampler.generate(state_vec) for _ in range(10)])
pick_base_poses = get_pick_base_poses(action, smples)
place_base_poses = get_place_base_poses(action, smples, mover)
utils.visualize_path(place_base_poses)
#utils.visualize_path(pick_base_poses)
import pdb;
pdb.set_trace()
smpled_param = smpler.sample_next_point(action, n_iter=200, n_parameters_to_try_motion_planning=3,
cached_collisions=state.collides,
cached_holding_collisions=None)
"""
# lowest valued items are retrieved first in PriorityQueue
action_queue = Queue.PriorityQueue(
) # (heuristic, nan, operator skeleton, state. trajectory);
initnode = Node(None, None, state)
initial_state = state
actions = get_actions(mover, goal, config)
nodes = [initnode]
for a in actions:
hval = compute_heuristic(state, a, pap_model, mover, config)
discrete_params = (a.discrete_parameters['object'],
a.discrete_parameters['region'])
initnode.set_heuristic(discrete_params, hval)
action_queue.put((hval, float('nan'), a, initnode)) # initial q
iter = 0
# beginning of the planner
while True:
iter += 1
curr_time = time.time() - tt
print "Time %.2f / %.2f " % (curr_time, config.timelimit)
print "Iter %d / %d" % (iter, config.num_node_limit)
if curr_time > config.timelimit or iter > config.num_node_limit:
return None, iter, nodes
if action_queue.empty():
actions = get_actions(mover, goal, config)
for a in actions:
discrete_params = (a.discrete_parameters['object'],
a.discrete_parameters['region'])
hval = initnode.heuristic_vals[discrete_params]
action_queue.put(
(hval, float('nan'), a, initnode)) # initial q
curr_hval, _, action, node = action_queue.get()
state = node.state
print "Curr hval", curr_hval
if node.depth > depth_limit:
print('skipping because depth limit',
node.action.discrete_parameters.values())
# reset to state
state.restore(mover)
if action.type == 'two_arm_pick_two_arm_place':
print("Sampling for {}".format(
action.discrete_parameters.values()))
if learned_smpler is None:
smpler = PaPUniformGenerator(action, mover, max_n_iter=200)
stime = time.time()
smpled_param = smpler.sample_next_point(
action,
n_parameters_to_try_motion_planning=3,
cached_collisions=state.collides,
cached_holding_collisions=None)
print 'smpling time', time.time() - stime
else:
smpler = LearnedGenerator(action,
mover,
learned_smpler,
state,
max_n_iter=200)
# How can I change the state.collides to the one_hot? How long would it take?
stime = time.time()
smpled_param = smpler.sample_next_point(
action,
n_parameters_to_try_motion_planning=3,
cached_collisions=state.collides,
cached_holding_collisions=None)
print 'smpling time', time.time() - stime
if smpled_param['is_feasible']:
action.continuous_parameters = smpled_param
action.execute()
print "Action executed"
else:
print "Failed to sample an action"
continue
is_goal_achieved = \
np.all([mover.regions['home_region'].contains(mover.env.GetKinBody(o).ComputeAABB()) for o in
obj_names[:n_objs_pack]])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
plan = list(node.backtrack()
)[::-1] # plan of length 0 is possible I think
plan = [nd.action for nd in plan[1:]] + [action]
return plan, iter, nodes
else:
newstate = statecls(mover, goal, node.state, action)
print "New state computed"
newnode = Node(node, action, newstate)
newactions = get_actions(mover, goal, config)
for newaction in newactions:
hval = compute_heuristic(newstate, newaction, pap_model,
mover,
config) - 1. * newnode.depth
action_queue.put((hval, float('nan'), newaction, newnode))
nodes.append(newnode)
elif action.type == 'one_arm_pick_one_arm_place':
print("Sampling for {}".format(
action.discrete_parameters.values()))
success = False
obj = action.discrete_parameters['object']
region = action.discrete_parameters['region']
o = obj.GetName()
r = region.name
if (o, r) in state.nocollision_place_op:
pick_op, place_op = node.state.nocollision_place_op[(o, r)]
pap_params = pick_op.continuous_parameters, place_op.continuous_parameters
else:
mover.enable_objects()
current_region = mover.get_region_containing(obj).name
papg = OneArmPaPUniformGenerator(
action,
mover,
cached_picks=(node.state.iksolutions[current_region],
node.state.iksolutions[r]))
pick_params, place_params, status = papg.sample_next_point(500)
if status == 'HasSolution':
pap_params = pick_params, place_params
else:
pap_params = None
if pap_params is not None:
pick_params, place_params = pap_params
action = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={
'object': obj,
'region': mover.regions[r],
},
continuous_parameters={
'pick': pick_params,
'place': place_params,
})
action.execute()
success = True
is_goal_achieved = \
np.all([mover.regions['rectangular_packing_box1_region'].contains(
mover.env.GetKinBody(o).ComputeAABB()) for o in obj_names[:n_objs_pack]])
if is_goal_achieved:
print("found successful plan: {}".format(n_objs_pack))
plan = list(node.backtrack()
)[::-1] # plan of length 0 is possible I think
plan = [nd.action for nd in plan[1:]] + [action]
return plan, iter, nodes
else:
newstate = statecls(mover, goal, node.state, action)
print "New state computed"
newnode = Node(node, action, newstate)
newactions = get_actions(mover, goal, config)
print "Old h value", curr_hval
for newaction in newactions:
hval = compute_heuristic(newstate, newaction,
pap_model, mover,
config) - 1. * newnode.depth
action_queue.put(
(hval, float('nan'), newaction, newnode))
if not success:
print('failed to execute action')
else:
print('action successful')
else:
raise NotImplementedError