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 compute_n_in_way_for_object_moved(self, object_moved, abs_state,
goal_objs):
goal_objs_not_in_goal = [
goal_obj for goal_obj in goal_objs if not abs_state.binary_edges[
(goal_obj, 'home_region')][0] and goal_obj != object_moved
] # Object can't be in the path to itself
n_in_way = 0
original_config = utils.get_robot_xytheta(abs_state.problem_env.robot)
for goal_obj in goal_objs_not_in_goal:
Vpre = abs_state.cached_pick_paths[goal_obj]
objects_in_way = [
o.GetName() for o in self.problem_env.get_objs_in_collision(
Vpre, 'entire_region')
]
is_object_moved_in_the_pick_path_to_goal_obj = object_moved in objects_in_way
n_in_way += is_object_moved_in_the_pick_path_to_goal_obj
for goal_obj in goal_objs_not_in_goal:
# Don't I include the pick path collisions?
pick = abs_state.pick_used[goal_obj]
pick.execute()
Vmanip = abs_state.cached_place_paths[(goal_obj, 'home_region')]
objects_in_way = [
o.GetName() for o in self.problem_env.get_objs_in_collision(
Vmanip, 'entire_region')
]
is_object_moved_in_the_place_path_for_goal_o_to_r = object_moved in objects_in_way
n_in_way += is_object_moved_in_the_place_path_for_goal_o_to_r
utils.two_arm_place_object(pick.continuous_parameters)
utils.set_robot_config(original_config)
return n_in_way
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_feasible_op_parameters(self,
operator_skeleton,
n_iter,
n_parameters_to_try_motion_planning,
chosen_pick_base_pose=None):
assert n_iter > 0
feasible_op_parameters = []
feasibility_check_time = 0
stime = time.time()
for i in range(n_iter):
# print 'Sampling attempts %d/%d' % (i, n_iter)
op_parameters = self.sample_from_uniform()
if 'pick' in self.operator_type:
if chosen_pick_base_pose is None:
self.op_feasibility_checker.action_mode = 'ir_parameters'
else:
op_parameters[3:] = utils.get_robot_xytheta()
self.op_feasibility_checker.action_mode = 'robot_base_pose'
self.tried_smpls.append(op_parameters)
stime2 = time.time()
op_parameters, status = self.op_feasibility_checker.check_feasibility(
operator_skeleton, op_parameters, self.swept_volume_constraint)
feasibility_check_time += time.time() - stime2
if self.op_feasibility_checker.action_mode == 'ir_parameters':
self.n_ik_checks += 1
if status != 'HasSolution':
self.n_ik_infeasible += 1
elif self.op_feasibility_checker.action_mode == 'robot_base_pose':
self.n_mp_checks += 1
if status != 'HasSolution':
self.n_mp_infeasible += 1
elif self.op_feasibility_checker.action_mode == 'object_pose':
self.n_mp_checks += 1
if status != 'HasSolution':
self.n_mp_infeasible += 1
else:
raise NotImplementedError
if status == 'HasSolution':
feasible_op_parameters.append(op_parameters)
if len(feasible_op_parameters
) >= n_parameters_to_try_motion_planning:
break
smpling_time = time.time() - stime
#print "Sampling time", smpling_time
#print "Feasibilty time", feasibility_check_time
if len(feasible_op_parameters) == 0:
feasible_op_parameters.append(op_parameters) # place holder
status = "NoSolution"
else:
status = "HasSolution"
return feasible_op_parameters, status
def make_vertices(self, qg, key_configs, collisions):
q0 = utils.get_robot_xytheta().squeeze()
if 'Relative' in self.pick_net.__class__.__name__:
rel_qg = compute_relative_config(q0[None, :], qg[None, :])
rel_qk = compute_relative_config(q0[None, :], key_configs)
repeat_qg = np.repeat(np.array(rel_qg), 618, axis=0)
v = np.hstack([rel_qk, repeat_qg, collisions])
else:
repeat_q0 = np.repeat(np.array(q0)[None, :], 618, axis=0)
repeat_qg = np.repeat(np.array(qg)[None, :], 618, axis=0)
v = np.hstack([key_configs, repeat_q0, repeat_qg, collisions])
v = v[None, :]
v = torch.from_numpy(v).float()
return v
def sample_qg(problem_env, region_name):
q0 = utils.get_robot_xytheta()
openrave_env = problem_env.env
place_domain = utils.get_place_domain(
region=problem_env.regions[region_name])
dim_parameters = place_domain.shape[-1]
domain_min = place_domain[0]
domain_max = place_domain[1]
qgs = np.random.uniform(domain_min, domain_max,
(500, dim_parameters)).squeeze()
for qg in qgs:
utils.set_robot_config(qg)
if not openrave_env.CheckCollision(problem_env.robot):
break
utils.set_robot_config(q0)
return qg
def compute_v_manip(abs_state, goal_objs):
goal_objs_not_in_goal = [
goal_obj for goal_obj in goal_objs
if not abs_state.binary_edges[(goal_obj, 'home_region')][0]
]
v_manip = np.zeros((len(abs_state.prm_vertices), 1))
# todo optimize this code
init_end_times = 0
path_times = 0
original_config = utils.get_robot_xytheta(abs_state.problem_env.robot)
stime = time.time()
for goal_obj in goal_objs_not_in_goal:
prm_path = abs_state.cached_place_paths[(goal_obj, 'home_region')]
stime2 = time.time()
# todo possible optimization:
# I can use the prm_path[1]'s edge to compute the neighbors, instead of using all of them.
distances = [
utils.base_pose_distance(prm_path[0], prm_vtx)
for prm_vtx in abs_state.prm_vertices
]
init_end_times += time.time() - stime2
closest_prm_idx = np.argmin(distances)
prm_path.pop(0)
prm_path.insert(0, abs_state.prm_vertices[closest_prm_idx, :])
stime2 = time.time()
distances = [
utils.base_pose_distance(prm_path[-1], prm_vtx)
for prm_vtx in abs_state.prm_vertices
]
init_end_times += time.time() - stime2
closest_prm_idx = np.argmin(distances)
prm_path[-1] = abs_state.prm_vertices[closest_prm_idx, :]
stime2 = time.time()
for p in prm_path:
boolean_matching_prm_vertices = np.all(np.isclose(
abs_state.prm_vertices[:, :2], p[:2]),
axis=-1)
if np.any(boolean_matching_prm_vertices):
idx = np.argmax(boolean_matching_prm_vertices)
v_manip[idx] = 1
path_times += time.time() - stime2
print 'v_manip creation time', time.time() - stime
utils.set_robot_config(original_config, robot=abs_state.problem_env.robot)
return v_manip
def make_vertices(qg, key_configs, collisions, net):
q0 = utils.get_robot_xytheta().squeeze()
if key_configs.shape[-1] == 4:
q0 = utils.encode_pose_with_sin_and_cos_angle(q0)
qg = utils.encode_pose_with_sin_and_cos_angle(qg)
repeat_q0 = np.repeat(np.array(q0)[None, :], 618, axis=0)
repeat_qg = np.repeat(np.array(qg)[None, :], 618, axis=0)
v = np.hstack([key_configs, repeat_q0, repeat_qg, collisions])
if 'Relative' in net.__class__.__name__:
rel_qg = compute_relative_config(q0[None, :], qg[None, :])
rel_qk = compute_relative_config(q0[None, :], key_configs)
repeat_qg = np.repeat(np.array(rel_qg), 618, axis=0)
v = np.hstack([rel_qk, repeat_qg, collisions])
else:
repeat_q0 = np.repeat(np.array(q0)[None, :], 618, axis=0)
repeat_qg = np.repeat(np.array(qg)[None, :], 618, axis=0)
v = np.hstack([key_configs, repeat_q0, repeat_qg, collisions])
# v = np.hstack([prm_vertices, repeat_q0, repeat_qg, self.collisions[idx]])
v = v[None, :]
v = torch.from_numpy(v).float()
return v
def predict(self, op_parameters, abstract_state, abstract_action):
collisions = self.process_abstract_state_collisions_into_key_config_obstacles(abstract_state)
key_configs = abstract_state.prm_vertices
pick_qg = op_parameters['pick']['q_goal']
v = self.make_vertices(pick_qg, key_configs, collisions)
is_pick_reachable = ((self.pick_net(v) > 0.5).cpu().numpy() == True)[0, 0]
if is_pick_reachable:
# I have to hold the object and check collisions
orig_config = utils.get_robot_xytheta()
target_obj = abstract_action.discrete_parameters['object']
utils.two_arm_pick_object(target_obj, {'q_goal': pick_qg})
collisions = utils.compute_occ_vec(key_configs)
collisions = utils.convert_binary_vec_to_one_hot(collisions)
utils.two_arm_place_object({'q_goal': pick_qg})
utils.set_robot_config(orig_config)
place_qg = op_parameters['place']['q_goal']
v = self.make_vertices(place_qg, key_configs, collisions)
pred = self.place_net(v)
is_place_reachable = ((pred > 0.5).cpu().numpy() == True)[0, 0]
return is_place_reachable
else:
return False