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 = UniformPaPGenerator(None,
operator_skeleton,
self.problem_env,
None,
n_candidate_params_to_smpl=3,
total_number_of_feasibility_checks=500,
dont_check_motion_existence=True)
# we should disable objects, because we are getting shortest path that ignors all collisions anyways
self.problem_env.disable_objects_in_region('entire_region')
op_cont_params, _ = generator.sample_candidate_params_with_increasing_iteration_limit()
motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None]
"""
for n_iter_to_try in n_iters:
op_cont_params, _ = generator.sample_feasible_op_parameters(operator_skeleton,
n_iter=n_iter_to_try,
n_parameters_to_try_motion_planning=5)
# I see. So here, return no op['q_goal'] when it is not feasible.
motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None]
if len(motion_plan_goals) > 2:
break
"""
self.problem_env.enable_objects_in_region('entire_region')
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['q_goal'] = motion_plan_goals # to make it consistent with Dpl
if len(motion_plan_goals) == 0:
import pdb;pdb.set_trace()
return operator_skeleton
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)
# we should disable objects, because we are getting shortest path that ignors all collisions anyways
self.problem_env.disable_objects_in_region('entire_region')
motion_plan_goals = []
n_iters = range(10, 500, 10)
for n_iter_to_try in n_iters:
op_cont_params, _ = generator.sample_feasible_op_parameters(
operator_skeleton,
n_iter=n_iter_to_try,
n_parameters_to_try_motion_planning=5)
motion_plan_goals = [
op['q_goal'] for op in op_cont_params
if op['q_goal'] is not None
]
if len(motion_plan_goals) > 2:
break
self.problem_env.enable_objects_in_region('entire_region')
# 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[
'q_goal'] = motion_plan_goals # to make it consistent with Dpl
return operator_skeleton
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_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 compute_and_cache_ik_solutions(self, ikcachename):
before = CustomStateSaver(self.problem_env.env)
utils.open_gripper()
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(False)
self.problem_env.disable_objects()
self.iksolutions = {}
o = u'c_obst0'
obj = self.problem_env.env.GetKinBody(o)
if True:
# for o, obj in self.objects.items():
# print(o)
self.iksolutions[o] = {r: [] for r in self.regions}
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj})
pick_generator = UniformGenerator(pick_op, self.problem_env)
pick_feasibility_checker = OneArmPickFeasibilityChecker(self.problem_env)
for _ in range(10000):
pick_params = pick_generator.sample_from_uniform()
iks = []
for r, region in self.regions.items():
place_op = Operator(operator_type='one_arm_place',
discrete_parameters={
'object': obj,
'region': region})
place_generator = UniformGenerator(place_op, self.problem_env)
status = 'NoSolution'
for _ in range(10):
obj_pose = place_generator.sample_from_uniform()
set_obj_xytheta(obj_pose, obj)
set_point(obj, np.hstack([obj_pose[0:2], region.z + 0.001]))
params, status = pick_feasibility_checker.check_feasibility(pick_op, pick_params)
if status == 'HasSolution':
iks.append((obj.GetTransform(), params))
break
if status == 'NoSolution':
break
if len(iks) == len(self.regions):
for r, ik in zip(self.regions, iks):
self.iksolutions[o][r].append(ik)
if all(len(self.iksolutions[o][r]) >= 1000 for r in self.regions):
break
# print([len(self.iksolutions[o][r]) for r in self.regions])
self.iksolutions = self.iksolutions[o]
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(True)
self.problem_env.enable_objects()
before.Restore()
pickle.dump(self.iksolutions, open(ikcachename, 'w'))
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 get_applicable_ops(self, parent_op=None):
applicable_ops = []
for op_name in self.operator_names:
if op_name.find('place') != -1:
if self.check_holding_object_precondition():
object_held = self.robot.GetGrabbed()[0]
if self.applicable_op_constraint is None:
for region in self.placement_regions.values():
if op_name == 'one_arm_place':
assert parent_op is not None
op = Operator(
operator_type=op_name,
discrete_parameters={
'region': region,
'object': object_held
},
continuous_parameters={
'grasp_params':
parent_op.
continuous_parameters['grasp_params']
})
else:
op = Operator(operator_type=op_name,
discrete_parameters={
'region': region,
'object': object_held
})
applicable_ops.append(op)
else:
op = Operator(
operator_type=op_name,
discrete_parameters={
'region':
self.applicable_op_constraint['region'],
'object': object_held
})
applicable_ops.append(op)
else:
if not self.check_holding_object_precondition():
if self.applicable_op_constraint is None:
for obj in self.objects:
op = Operator(operator_type=op_name,
discrete_parameters={'object': obj})
applicable_ops.append(op)
else:
op = Operator(
operator_type=op_name,
discrete_parameters={
'object':
self.applicable_op_constraint['object']
})
applicable_ops.append(op)
return applicable_ops
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 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_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,
'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),
'region': self.problem_env.regions[r]
})
papg = OneArmPaPUniformGenerator(
op_skel,
self.problem_env,
cached_picks=(self.iksolutions[current_region],
self.iksolutions[r]))
num_tries = 200
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')
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 add_actions(self, action):
new_action = Operator(
operator_type=self.operator_skeleton.type,
discrete_parameters=self.operator_skeleton.discrete_parameters,
continuous_parameters=action)
self.A.append(new_action)
self.N[new_action] = 0
def get_uniform_sampler_place_feasibility_rate(pick_smpls, place_smpls,
target_obj, problem_env):
feasibility_checker = two_arm_pap_feasiblity_checker.TwoArmPaPFeasibilityChecker(
problem_env)
op = Operator('two_arm_place', {
"object": target_obj,
"place_region": 'loading_region'
})
n_success = 0
orig_xytheta = utils.get_robot_xytheta(problem_env.robot)
for pick_smpl, place_smpl in zip(pick_smpls, place_smpls):
parameters = np.hstack([pick_smpl, place_smpl])
param, status = feasibility_checker.check_feasibility(
op,
parameters,
swept_volume_to_avoid=None,
parameter_mode='obj_pose')
if status == 'HasSolution':
motion, status = problem_env.motion_planner.get_motion_plan(
[param['pick']['q_goal']], cached_collisions=None)
if status == 'HasSolution':
utils.two_arm_pick_object(target_obj, param['pick'])
motion, status = problem_env.motion_planner.get_motion_plan(
[param['place']['q_goal']], cached_collisions=None)
utils.two_arm_place_object(param['pick'])
utils.set_robot_config(orig_xytheta, problem_env.robot)
n_success += status == 'HasSolution'
total_samples = len(pick_smpls)
return n_success / float(total_samples) * 100
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 add_actions(self, continuous_parameters):
new_action = Operator(
operator_type=self.operator_skeleton.type,
discrete_parameters=self.operator_skeleton.discrete_parameters,
continuous_parameters=continuous_parameters,
low_level_motion=None)
self.A.append(new_action)
self.N[new_action] = 0
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 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_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_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 get_applicable_op_skeleton(self, parent_action):
if parent_action is None:
op_name = 'two_arm_pick'
else:
# use_multipaps = parent_action.discrete_parameters['object'].GetName().find("big") != -1
use_multipaps = self.n_actions_per_node > 1
if parent_action.type == 'two_arm_pick':
if use_multipaps:
op_name = str(self.n_actions_per_node) + '_paps'
else:
op_name = 'two_arm_place'
else:
op_name = 'two_arm_pick'
if op_name == 'two_arm_place':
op = Operator(operator_type=op_name,
discrete_parameters={
'region': self.regions['object_region'],
'object': self.objects_currently_not_in_goal[0]
},
continuous_parameters=None,
low_level_motion=None)
elif op_name == 'two_arm_pick':
op = Operator(operator_type=op_name,
discrete_parameters={
'object': self.objects_currently_not_in_goal[0]
},
continuous_parameters=None,
low_level_motion=None)
elif op_name.find('_paps') != -1:
objects = self.objects_currently_not_in_goal[0:self.
n_actions_per_node]
op = Operator(operator_type=op_name,
discrete_parameters={
'objects': objects,
'region': self.regions['object_region']
},
continuous_parameters=None,
low_level_motion=None)
else:
raise NotImplementedError
return op
def get_applicable_op_skeleton(self, parent_action):
op_name = self.which_operator()
if parent_action is None:
op_name = 'two_arm_pick'
else:
if parent_action.type == 'two_arm_pick':
op_name = 'two_arm_place'
else:
op_name = 'two_arm_pick'
if op_name == 'two_arm_place':
op = Operator(operator_type=op_name,
discrete_parameters={'region': self.regions['entire_region']},
continuous_parameters=None,
low_level_motion=None)
else:
op = Operator(operator_type=op_name,
discrete_parameters={'object': self.objects_currently_not_in_goal[0]},
continuous_parameters=None,
low_level_motion=None)
return op
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 get_pap_used_in_plan(self, plan):
obj_to_pick = {op.discrete_parameters['object']: [] for op in plan}
obj_to_place = {(op.discrete_parameters['object'],
op.discrete_parameters['place_region']): []
for op in plan}
for op in plan:
# making obj_to_pick and obj_to_place used in the plan; this can be done once, not every time
pick_cont_param = op.continuous_parameters['pick']
pick_disc_param = {'object': op.discrete_parameters['object']}
pick_op = Operator('two_arm_pick', pick_disc_param,
pick_cont_param)
pick_op.low_level_motion = pick_cont_param['motion']
obj_to_pick[op.discrete_parameters['object']].append(pick_op)
place_cont_param = op.continuous_parameters['place']
place_disc_param = {
'object': op.discrete_parameters['object'],
'place_region': op.discrete_parameters['place_region']
}
place_op = Operator('two_arm_pick', place_disc_param,
place_cont_param)
place_op.low_level_motion = place_cont_param['motion']
obj_to_place[(
op.discrete_parameters['object'],
op.discrete_parameters['place_region'])].append(place_op)
return [obj_to_pick, obj_to_place]
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 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 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,
'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
