def get_pap_pick_place_params(self, obj, region, swept_volume):
stime = time.time()
r = region
print(obj, r)
current_region = self.problem_env.get_region_containing(obj).name
# check existing solutions
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj})
place_op = Operator(operator_type='one_arm_place', discrete_parameters={'object': obj,
'place_region':
self.problem_env.regions[r]})
obj_kinbody = self.problem_env.env.GetKinBody(obj)
if len(self.pap_params[(obj, r)]) > 0:
for pick_params, place_params in self.pap_params[(obj, r)]:
pick_op.continuous_parameters = pick_params
place_op.continuous_parameters = place_params
if not self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume):
return pick_params, place_params
op_skel = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'place_region': self.problem_env.regions[r]})
# papg = OneArmPaPUniformGenerator(op_skel,
# self.problem_env,
# cached_picks=None)
sampler = {'pick': UniformSampler(target_region=None, atype='one_arm_pick'),
'place': UniformSampler(target_region=self.problem_env.regions[r], atype='one_arm_place')}
papg = OneArmPaPGenerator(op_skel, n_iter_limit=self.config.n_iter_limit,
problem_env=self.problem_env,
pick_sampler=sampler['pick'], place_sampler=sampler['place'])
pick_params, place_params, status = papg.sample_next_point()
if 'HasSolution' in status:
self.pap_params[(obj, r)].append((pick_params, place_params))
self.pick_params[obj].append(pick_params)
print('success')
pick_op.continuous_parameters = pick_params
place_op.continuous_parameters = place_params
collision = self.check_collision_in_pap(pick_op, place_op, obj_kinbody, swept_volume)
if not collision:
print('found nocollision', obj, r)
return pick_params, place_params
return None, None
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_obj_reachable(self, obj):
if len(self.problem_env.robot.GetGrabbed()) > 0:
return False
obj = self.problem_env.env.GetKinBody(obj)
obj_name = str(obj.GetName())
if self.problem_env.name.find('one_arm') != -1:
op = Operator('one_arm_pick', {'object': obj})
else:
op = Operator('two_arm_pick', {'object': obj})
if obj_name in self.pick_used:
motion_plan_goals = self.pick_used[obj_name].continuous_parameters['q_goal']
else:
motion_plan_goals = self.generate_potential_pick_configs(op, n_pick_configs=10)
if motion_plan_goals is not None:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
motion, status = motion_planner.get_motion_plan(motion_plan_goals)
is_feasible_param = status == 'HasSolution'
else:
is_feasible_param = False
if is_feasible_param:
op.make_pklable()
op.continuous_parameters = {'q_goal': motion[-1]}
self.motion_plans[obj_name] = motion
if obj_name not in self.pick_used:
self.pick_used[obj_name] = op
self.evaluations[obj_name] = True
return True
else:
self.evaluations[obj_name] = False
return False
def sample_op_instance(self, curr_obj, region, swept_volume, n_iter):
op = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={
'object': curr_obj,
'region': region
})
# use the following:
# papg = OneArmPaPUniformGenerator(op_skel, self.problem_env,
# cached_picks=(self.iksolutions[current_region], self.iksolutions[r]))
pick_cont_param, place_cont_param = self.get_pap_pick_place_params(
curr_obj.GetName(), region.name, swept_volume)
#generator = OneArmPaPUniformGenerator(op, self.problem_env, swept_volume)
#pick_cont_param, place_cont_param, status = generator.sample_next_point(max_ik_attempts=n_iter)
if pick_cont_param is not None:
status = 'HasSolution'
else:
status = 'NoSolution'
op.continuous_parameters = {
'pick': pick_cont_param,
'place': place_cont_param
}
return op, status
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_op_instance(self, curr_obj, n_iter):
op = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={'object': curr_obj, 'region': self.goal_region})
target_object = op.discrete_parameters['object']
generator = OneArmPaPUniformGenerator(op, self.problem_env, None)
print "Sampling paps for ", target_object
pick_cont_param, place_cont_param, status = generator.sample_next_point(max_ik_attempts=n_iter)
op.continuous_parameters = {'pick': pick_cont_param, 'place': place_cont_param}
return op, status
def __call__(self, a, b, r, cached_path=None):
assert r in self.problem_env.regions
# While transferring "a" to region "r", is "b" in the way to region
if (a, b, r) in self.evaluations.keys():
return self.evaluations[(a, b, r)]
else:
# what happens a is already in r? We still plan a path, because we want to know if we can move object a
# inside region r
is_a_obj = a not in self.problem_env.region_names
is_b_obj = b not in self.problem_env.region_names
is_r_region = r in self.problem_env.region_names # this is already defended
if not is_a_obj or not is_b_obj:
return False
if a == b:
return False
if not is_r_region:
return False
min_constraint_path_already_computed = (a, r) in self.mc_to_entity.keys()
if min_constraint_path_already_computed:
objs_in_collision = self.mc_to_entity[(a, r)]
else:
saver = CustomStateSaver(self.problem_env.env)
if cached_path is not None:
self.pick_used[a].execute()
path = cached_path
status = 'HasSolution'
place_op = Operator(operator_type='two_arm_place', discrete_parameters={
'object': self.problem_env.env.GetKinBody(a),
'region': self.problem_env.regions[r],
})
place_op.low_level_motion = path
place_op.continuous_parameters = {'q_goal': path[-1]}
else:
self.pick_object(a)
path, status, place_op = self.plan_minimum_constraint_path_to_region(r)
if status != 'HasSolution':
objs_in_collision = None
else:
objs_in_collision = self.problem_env.get_objs_in_collision(path, 'entire_region')
objs_in_collision = [o.GetName() for o in objs_in_collision]
self.mc_to_entity[(a, r)] = objs_in_collision
self.mc_path_to_entity[(a, r)] = path
if len(objs_in_collision) == 0:
self.reachable_obj_region_pairs.append((a, r))
saver.Restore()
evaluation = objs_in_collision is not None and b in objs_in_collision
self.evaluations[(a, b, r)] = evaluation
return evaluation
def find_pick(self, curr_obj):
if self.problem_env.name.find("one_arm") != -1:
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': curr_obj})
else:
pick_op = Operator(operator_type='two_arm_pick', discrete_parameters={'object': curr_obj})
params = self.sample_cont_params(pick_op, n_iter=500)
if not params['is_feasible']:
return None
pick_op.continuous_parameters = params
return pick_op
def find_pap(self, curr_obj):
if self.problem_env.name.find("one_arm") != -1:
op = Operator(operator_type='one_arm_pick_one_arm_place',
discrete_parameters={
'object': curr_obj,
'place_region': self.goal_region.name
})
current_region = problem.get_region_containing(
problem.env.GetKinBody(o)).name
sampler = OneArmPaPUniformGenerator(
action,
problem,
cached_picks=(iksolutions[current_region],
self.iksolutions[self.goal_region.name]))
pick_params, place_params, status = sampler.sample_next_point(
self.config.n_iter_limit)
self.n_ik += generator.n_ik_checks
if status == 'HasSolution':
params = {'pick': pick_params, 'place': place_params}
else:
params = None
else:
op = Operator(operator_type='two_arm_pick_two_arm_place',
discrete_parameters={
'object': curr_obj,
'place_region': self.goal_region.name
})
state = None
sampler = UniformSampler(op.type, self.goal_region)
generator = TwoArmPaPGenerator(
state,
op,
sampler,
n_parameters_to_try_motion_planning=self.config.n_mp_limit,
n_iter_limit=self.config.n_iter_limit,
problem_env=self.problem_env,
pick_action_mode='ir_parameters',
place_action_mode='object_pose')
params = generator.sample_next_point()
self.n_mp += generator.n_mp_checks
self.n_ik += generator.n_ik_checks
# it must be because sampling a feasible pick can be done by trying as many as possible,
# but placements cannot be made feasible by sampling more
# also, it takes longer to check feasibility on place?
# I just have to check the IK solution once
if not params['is_feasible']:
return None
op.continuous_parameters = params
return op
def plan_pick_motion_for(self, object_to_move, pick_op_instance):
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': object_to_move})
motion_planner = MinimumConstraintPlanner(self.problem_env,
object_to_move, 'prm')
motion, status = motion_planner.get_motion_plan(
pick_op_instance.continuous_parameters['q_goal'])
if motion is None:
return None, "NoSolution", None
motion.append(pick_op_instance.continuous_parameters['q_goal'])
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, pick_op
def plan_minimum_constraint_path_to_region(self, region):
obj_holding = self.robot.GetGrabbed()[0]
target_region = self.problem_env.regions[region]
place_op = Operator(operator_type='two_arm_place',
discrete_parameters={
'object': obj_holding,
'place_region': target_region
})
obj_region_key = (obj_holding.GetName(), region)
if obj_region_key in self.mc_paths:
motion = self.mc_paths[(obj_holding.GetName(), region)]
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, 'HasSolution', place_op
if obj_region_key in self.place_used:
print "Using the place data" # todo but this depends on which state...
motion = self.place_used[obj_region_key].low_level_motion
status = 'HasSolution'
else:
potential_motion_plan_goals = self.generate_potential_place_configs(
place_op, n_pick_configs=10)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
self.mc_calls += 1
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj_holding)
if motion is None:
return None, "NoSolution", None
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
if obj_region_key not in self.place_used:
self.place_used[obj_region_key] = place_op
return motion, status, place_op
def get_feasible_pick(problem_env, target_obj):
pick_domain = utils.get_pick_domain()
dim_parameters = pick_domain.shape[-1]
domain_min = pick_domain[0]
domain_max = pick_domain[1]
smpls = np.random.uniform(domain_min, domain_max, (500, dim_parameters)).squeeze()
feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker(problem_env)
op = Operator('two_arm_pick', {"object": target_obj})
for smpl in smpls:
pick_param, status = feasibility_checker.check_feasibility(op, smpl, parameter_mode='ir_params')
if status == 'HasSolution':
op.continuous_parameters = pick_param
return op
def get_feasible_pick(problem_env, smpls):
global target_obj_name
feasibility_checker = two_arm_pick_feasibility_checker.TwoArmPickFeasibilityChecker(
problem_env)
op = Operator('two_arm_pick', {"object": target_obj_name})
for idx, param in enumerate(smpls):
feasible_pick, status = feasibility_checker.check_feasibility(
op, param, parameter_mode='ir_params')
if status == 'HasSolution':
print idx
if idx == 98: continue # idx=110
op.continuous_parameters = feasible_pick
break
return op
def find_place(self, curr_obj, pick):
if self.problem_env.name.find("one_arm") != -1:
place_op = Operator(operator_type='one_arm_place',
discrete_parameters={'object': curr_obj, 'region': self.goal_region},
continuous_parameters=pick.continuous_parameters)
else:
place_op = Operator(operator_type='two_arm_place', discrete_parameters={'object': curr_obj,
'region': self.goal_region})
# it must be because sampling a feasible pick can be done by trying as many as possible,
# but placements cannot be made feasible by sampling more
# also, it takes longer to check feasibility on place?
# I just have to check the IK solution once
params = self.sample_cont_params(place_op, n_iter=500)
if not params['is_feasible']:
return None
place_op.continuous_parameters = params
return place_op
def get_pick_from_initial_config(self, obj):
utils.set_robot_config(self.problem_env.initial_robot_base_pose)
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': obj})
potential_motion_plan_goals = self.generate_motion_plan_goals(
pick_op, n_configs=5)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj)
if motion is None:
return None, "NoSolution", None
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {
'q_goal': motion[-1],
'motion': motion
}
return motion, status, pick_op
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 plan_place(self, target_region, swept_volumes):
obj_holding = self.robot.GetGrabbed()[0]
place_op = Operator(operator_type='two_arm_place',
discrete_parameters={
'object': obj_holding,
'region': target_region
})
stime = time.time()
potential_motion_plan_goals = self.generate_motion_plan_goals(
place_op, n_configs=5, swept_volumes=swept_volumes)
print time.time() - stime
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj_holding)
if motion is None:
return None, "NoSolution", None
place_op.low_level_motion = motion
place_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, place_op
def get_pick_from_initial_config(self, obj):
utils.set_robot_config(self.problem_env.initial_robot_base_pose)
pick_op = Operator(operator_type='two_arm_pick',
discrete_parameters={'object': obj})
we_already_have_pick_config = obj.GetName(
) in self.sampled_pick_configs_for_objects.keys()
if we_already_have_pick_config:
return self.sampled_pick_configs_for_objects[obj.GetName()]
else:
potential_motion_plan_goals = self.generate_motion_plan_goals(
pick_op, n_configs=5)
if potential_motion_plan_goals is None:
return None, "NoSolution", None
motion, status = self.get_minimum_constraint_path_to(
potential_motion_plan_goals, obj)
if motion is None:
return None, "NoSolution", None
pick_op.low_level_motion = motion
pick_op.continuous_parameters = {'q_goal': motion[-1]}
return motion, status, pick_op
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 generate_smpls(problem_env, sampler, plan):
# make a prediction
# Make a feasible pick sample for the target object
idx = 0
plan_action = plan[0]
while True:
if plan_action.discrete_parameters['place_region'] == 'home_region':
utils.set_obj_xytheta(
plan_action.continuous_parameters['place']['object_pose'],
plan_action.discrete_parameters['object'])
else:
if plan_action.discrete_parameters[
'object'] == 'rectangular_packing_box2':
break
else:
utils.set_obj_xytheta(
plan_action.continuous_parameters['place']['object_pose'],
plan_action.discrete_parameters['object'])
idx += 1
plan_action = plan[idx]
import pdb
pdb.set_trace()
target_obj_name = plan_action.discrete_parameters['object']
place_region = 'loading_region'
abstract_action = Operator('two_arm_pick_two_arm_place', {
'object': target_obj_name,
'place_region': place_region
})
abstract_action.continuous_parameters = plan_action.continuous_parameters
pick_base_pose = plan_action.continuous_parameters['pick']['q_goal']
abstract_action.execute_pick()
import pdb
pdb.set_trace()
utils.set_robot_config(
plan_action.continuous_parameters['place']['q_goal'])
goal_entities = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region'
]
sampler_state = ConcreteNodeState(problem_env, target_obj_name,
place_region, goal_entities)
inp = make_input_for_place(sampler_state, pick_base_pose)
key_configs = pickle.load(open('prm.pkl', 'r'))[0]
cols = inp['collisions'].squeeze()
colliding_idxs = np.where(cols[:, 1] == 0)[0]
colliding_key_configs = key_configs[colliding_idxs, :]
samples = []
values = []
for _ in range(20):
sample = sampler.sample_from_voo(inp['collisions'],
inp['poses'],
voo_iter=50,
colliding_key_configs=None,
tried_samples=np.array([]))
values.append(
sampler.value_network.predict(
[sample[None, :], inp['collisions'], inp['poses']])[0, 0])
sample = utils.decode_pose_with_sin_and_cos_angle(sample)
samples.append(sample)
utils.visualize_path(samples)
print values
# visualize_samples(samples, problem_env, target_obj_name)
# visualize_samples([samples[np.argmax(values)]], problem_env, target_obj_name)
return samples
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)
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 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()
