def sample_placements(self, pose_ids, collisions, n_smpls):
if self.v_manip is None:
v_manip = compute_v_manip(self.abstract_state,
self.abstract_state.goal_entities[:-1],
self.key_configs)
v_manip = utils.convert_binary_vec_to_one_hot(
v_manip.squeeze()).reshape((1, len(self.key_configs), 2, 1))
v_manip = np.tile(v_manip, (n_smpls, 1, 1, 1))
self.v_manip = v_manip
state_vec = np.concatenate([collisions, self.v_manip], axis=2)
if 'center_shelf' in self.region:
chosen_sampler = self.samplers['place_obj_region']
place_samples = chosen_sampler.generate(state_vec, pose_ids)
place_samples = np.array([
utils.decode_pose_with_sin_and_cos_angle(s)
for s in place_samples
])
elif 'rectangular_packing_box1_region' in self.region:
chosen_sampler = self.samplers['place_goal_region']
place_samples = np.array(
[chosen_sampler.sample() for _ in range(n_smpls)])
else:
raise NotImplementedError
return place_samples
def get_unprocessed_placement(placement, obj_abs_pose):
placement = utils.decode_pose_with_sin_and_cos_angle(placement)
if action_data_mode == 'pick_parameters_place_relative_to_object':
#abs_place = placement.squeeze() + obj_abs_pose.squeeze()
abs_place = utils.get_absolute_pose_from_relative_pose(
placement, obj_abs_pose)
else:
raise NotImplementedError
return abs_place
def unprocess_pick_and_place_smpls(smpls):
pick_smpls = smpls[0]
place_smpls = smpls[1]
pick_unprocessed = []
place_unprocessed = []
for pick_smpl, place_smpl in zip(pick_smpls, place_smpls):
grasp_params = pick_smpl[0:3]
ir_parameters = pick_smpl[3:]
portion = ir_parameters[0]
base_angle = utils.decode_sin_and_cos_to_angle(ir_parameters[1:3])
facing_angle_offset = ir_parameters[3]
pick_unprocessed.append(
np.hstack([grasp_params, portion, base_angle,
facing_angle_offset]))
abs_base_pose = utils.decode_pose_with_sin_and_cos_angle(place_smpl)
place_unprocessed.append(abs_base_pose)
smpls = np.hstack([pick_unprocessed, place_unprocessed])
return smpls
def sample_placements(self, pose_ids, collisions, pick_samples, n_smpls):
stttt = time.time()
obj_kinbody = self.abstract_state.problem_env.env.GetKinBody(self.obj)
stime = time.time()
obj_xyth = utils.get_body_xytheta(obj_kinbody)
print 'objxytheta time', time.time() - stime
stime = time.time()
pick_abs_poses = []
for s in pick_samples:
_, poses = utils.get_pick_base_pose_and_grasp_from_pick_parameters(
obj_kinbody, s, obj_xyth)
pick_abs_poses.append(poses)
print "Pick abs pose time", time.time() - stime
stime = time.time()
encoded_pick_abs_poses = np.array([
utils.encode_pose_with_sin_and_cos_angle(s) for s in pick_abs_poses
])
print "Pick pose encoding time", time.time() - stime
pose_ids[:, -6:-2] = encoded_pick_abs_poses
if 'home' in self.region:
chosen_sampler = self.samplers['place_goal_region']
else:
chosen_sampler = self.samplers['place_obj_region']
stime = time.time()
place_samples = chosen_sampler.generate(self.state_vec, pose_ids)
print "prediction time", time.time() - stime
stime = time.time()
place_samples = np.array([
utils.decode_pose_with_sin_and_cos_angle(s) for s in place_samples
])
print "place decoding time", time.time() - stime
# print time.time() - stttt
return place_samples
def main():
problem_seed = 1
states, konf_relevance, poses, rel_konfs, goal_flags, actions, sum_rewards = \
get_data('n_objs_pack_4', 'place', 'loading_region')
actions = actions[:, -4:]
actions = [utils.decode_pose_with_sin_and_cos_angle(a) for a in actions]
problem_env, openrave_env = create_environment(problem_seed)
key_configs = pickle.load(open('prm.pkl', 'r'))[0]
key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0)
indices_to_delete = sampler_utils.get_indices_to_delete(
'loading_region', key_configs)
key_configs = np.delete(key_configs, indices_to_delete, axis=0)
filtered_konfs = filter_configs_that_are_too_close(actions)
import pdb
pdb.set_trace()
utils.viewer()
utils.visualize_placements(filtered_konfs, 'square_packing_box1')
import pdb
pdb.set_trace()
def sample_new_points(self, n_smpls):
poses = data_processing_utils.get_processed_poses_from_state(self.smpler_state, None)[None, :]
# sample picks
pick_min = get_pick_domain()[0]
pick_max = get_pick_domain()[1]
pick_samples = np.random.uniform(pick_min, pick_max, (1, 6)).squeeze()
# todo change it to generate how many ever pick samples there are
raise NotImplementedError
must_get_q0_from_pick_abs_pose = action_data_mode == 'PICK_grasp_params_and_abs_base_PLACE_abs_base'
assert must_get_q0_from_pick_abs_pose
pick_abs_poses = pick_samples[3:7]
pick_abs_poses = utils.encode_pose_with_sin_and_cos_angle(pick_abs_poses)
poses[:, -4:] = pick_abs_poses
# Here, it would be much more accurate if I use place collision vector, but at this point
# I don't know if the pick is feasible. Presumably, we can check the feasbility based on pick first, and
# only if that is feasible, move onto a place. But this gets ugly as to how to "count" the number of samples
# tried. I guess if we count the pick trials, it is same as now?
collisions = self.smpler_state.pick_collision_vector
samples = self.policy.generate(collisions, poses, n_data=n_smpls)
samples = np.array([utils.decode_pose_with_sin_and_cos_angle(s) for s in samples])
import pdb;pdb.set_trace()
return samples
def unprocess_place_smpls(smpls):
unprocessed = []
for smpl in smpls:
abs_base_pose = utils.decode_pose_with_sin_and_cos_angle(smpl)
unprocessed.append(abs_base_pose)
return np.array(unprocessed)
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
