def get_processed_poses_from_action(state, action, action_data_mode):
# grasp_params abs_pick_pose
# grasp params ir_parameters
# 'PICK_grasp_params_and_abs_base_PLACE_abs_base'
if 'PICK_grasp_params_and_abs_base' in action_data_mode:
grasp_params = action['pick_action_parameters'][0:3][None, :]
abs_pick_pose = utils.encode_pose_with_sin_and_cos_angle(
action['pick_abs_base_pose'])[None, :]
pick_params = np.hstack([grasp_params, abs_pick_pose])
elif 'PICK_grasp_params_and_ir_parameters' in action_data_mode:
abs_pick_pose = action['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(abs_pick_pose, state.abs_obj_pose)
base_angle = utils.encode_angle_in_sin_and_cos(base_angle)
grasp_params = action['pick_action_parameters'][0:3]
pick_params = np.hstack(
[grasp_params, portion, base_angle, facing_angle_offset])[None, :]
else:
raise NotImplementedError
if 'PLACE_abs_base' in action_data_mode:
place_params = utils.encode_pose_with_sin_and_cos_angle(
action['place_abs_base_pose'])[None, :]
else:
raise NotImplementedError
action = np.hstack([pick_params, place_params])
return action
def prepare_input(smpler_state,
noise_batch,
delete=False,
region=None,
filter_konfs=False):
poses = data_processing_utils.get_processed_poses_from_state(
smpler_state, None)[None, :]
obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose)
smpler_state.abs_obj_pose = obj_pose
goal_flags = smpler_state.goal_flags
collisions = smpler_state.pick_collision_vector
key_configs = pickle.load(open('prm.pkl', 'r'))[0]
key_configs = np.array(
[utils.encode_pose_with_sin_and_cos_angle(p) for p in key_configs])
key_configs = key_configs.reshape((1, len(key_configs), 4, 1))
key_configs = key_configs.repeat(len(poses), axis=0)
n_smpls = len(noise_batch)
goal_flags = np.tile(goal_flags, (n_smpls, 1, 1, 1))
key_configs = np.tile(key_configs, (n_smpls, 1, 1, 1))
collisions = np.tile(collisions, (n_smpls, 1, 1, 1))
poses = np.tile(poses, (n_smpls, 1))
if len(noise_batch) > 1:
noise_batch = np.array(noise_batch).squeeze()
inp = [goal_flags, key_configs, collisions, poses, noise_batch]
return inp
def make_input_for_place(smpler_state, pick_base_pose):
inp = prepare_input_except_noise(smpler_state,
delete=True,
region='loading_region',
filter_konfs=False)
poses = inp['poses']
poses[:, -4:] = utils.encode_pose_with_sin_and_cos_angle(pick_base_pose)
inp['poses'] = poses
return inp
def make_konfs_relative_to_pose(obj_pose, key_configs):
rel_konfs = []
utils.clean_pose_data(obj_pose)
for k in key_configs:
konf = utils.clean_pose_data(k)
rel_konf = utils.get_relative_robot_pose_wrt_body_pose(konf, obj_pose)
rel_konf = utils.encode_pose_with_sin_and_cos_angle(rel_konf)
rel_konfs.append(rel_konf)
return np.array(rel_konfs)
def get_processed_poses_from_state(state):
if state_data_mode == 'absolute':
obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose)
robot_pose = utils.encode_pose_with_sin_and_cos_angle(state.robot_pose)
goal_obj_pose = utils.encode_pose_with_sin_and_cos_angle(
state.abs_goal_obj_pose)
elif state_data_mode == 'robot_rel_to_obj':
obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose)
robot_pose = utils.get_relative_robot_pose_wrt_body_pose(
state.abs_robot_pose, state.abs_obj_pose)
robot_pose = utils.encode_pose_with_sin_and_cos_angle(robot_pose)
goal_obj_pose = utils.get_relative_robot_pose_wrt_body_pose(
state.abs_goal_obj_pose, state.abs_obj_pose)
recovered = utils.clean_pose_data(
utils.get_absolute_pose_from_relative_pose(
goal_obj_pose, state.abs_obj_pose.squeeze()))
assert np.all(np.isclose(recovered, state.abs_goal_obj_pose.squeeze()))
goal_obj_pose = utils.encode_pose_with_sin_and_cos_angle(goal_obj_pose)
else:
raise not NotImplementedError
pose = np.hstack([obj_pose, goal_obj_pose, robot_pose])
return pose
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 get_processed_poses_from_state(state, state_data_mode):
obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose)
if state.abs_robot_pose.shape[1] == 11:
curr_robot_pose = np.array([-9999, -9999, -9999, -9999])
else:
curr_robot_pose = utils.encode_pose_with_sin_and_cos_angle(
state.abs_robot_pose)
#goal_obj_poses = np.hstack([utils.encode_pose_with_sin_and_cos_angle(o) for o in state.abs_goal_obj_poses]):w
goal_obj_poses = np.array([-9999, -9999, -9999, -9999])
"""
elif state_data_mode == 'robot_rel_to_obj':
obj_pose = utils.encode_pose_with_sin_and_cos_angle(state.abs_obj_pose)
robot_pose = utils.get_relative_robot_pose_wrt_body_pose(state.abs_robot_pose, state.abs_obj_pose)
curr_robot_pose = utils.encode_pose_with_sin_and_cos_angle(robot_pose)
goal_obj_poses = [utils.get_relative_robot_pose_wrt_body_pose(o, state.abs_obj_pose) for o in
state.abs_goal_obj_poses]
goal_obj_poses = [utils.encode_pose_with_sin_and_cos_angle(o) for o in goal_obj_poses]
goal_obj_poses = np.hstack(goal_obj_poses)
else:
raise not NotImplementedError
"""
pose = np.hstack([obj_pose, goal_obj_poses, curr_robot_pose])
return pose
def get_augmented_state_vec_and_poses(obj, state_vec, smpler_state):
n_key_configs = 615
utils.set_color(obj, [1, 0, 0])
is_goal_obj = utils.convert_binary_vec_to_one_hot(
np.array([obj in smpler_state.goal_entities]))
is_goal_obj = np.tile(is_goal_obj, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
is_goal_region = utils.convert_binary_vec_to_one_hot(
np.array([smpler_state.region in smpler_state.goal_entities]))
is_goal_region = np.tile(is_goal_region, (n_key_configs, 1)).reshape(
(1, n_key_configs, 2, 1))
state_vec = np.concatenate([state_vec, is_goal_obj, is_goal_region],
axis=2)
poses = np.hstack([
utils.encode_pose_with_sin_and_cos_angle(
utils.get_body_xytheta(obj).squeeze()), 0, 0, 0, 0
]).reshape((1, 8))
return state_vec, poses
def generate_pick_and_place_batch(smpler_state, policy, noise_batch):
pick_smpler = policy['pick']
inp = prepare_input(smpler_state,
noise_batch,
delete=True,
region='loading_region',
filter_konfs=False)
pick_samples = pick_smpler.policy_model.predict(inp)
pick_base_poses = []
for p in pick_samples:
ir_parameters = p[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_param = np.hstack(
[p[:3], portion, base_angle, facing_angle_offset])
_, pick_base_pose = utils.get_pick_base_pose_and_grasp_from_pick_parameters(
smpler_state.obj, pick_param)
pick_base_pose = utils.encode_pose_with_sin_and_cos_angle(
pick_base_pose)
pick_base_poses.append(pick_base_pose)
pick_base_poses = np.array(pick_base_poses)
inp = prepare_input(smpler_state,
noise_batch,
delete=True,
region=smpler_state.region,
filter_konfs=False)
poses = inp[-2]
poses[:, -4:] = pick_base_poses
inp[-2] = poses
z_smpls = gaussian_noise(z_size=(len(noise_batch), 4))
inp[-1] = z_smpls
place_smpler = policy['place']
place_samples = place_smpler.policy_model.predict(inp)
return pick_samples, place_samples
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 prepare_input_except_noise(smpler_state,
delete=False,
region=None,
filter_konfs=False):
poses = data_processing_utils.get_processed_poses_from_state(
smpler_state, None)[None, :]
obj_pose = utils.clean_pose_data(smpler_state.abs_obj_pose)
smpler_state.abs_obj_pose = obj_pose
goal_flags = smpler_state.goal_flags
collisions = smpler_state.pick_collision_vector
key_configs = pickle.load(open('prm.pkl', 'r'))[0]
"""
key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0)
if delete:
indices_to_delete = get_indices_to_delete(region, key_configs)
key_configs = np.delete(key_configs, indices_to_delete, axis=0)
collisions = np.delete(collisions, indices_to_delete, axis=1)
goal_flags = np.delete(goal_flags, indices_to_delete, axis=1)
if filter_konfs:
key_configs = data_processing_utils.filter_configs_that_are_too_close(key_configs)
"""
key_configs = np.array(
[utils.encode_pose_with_sin_and_cos_angle(p) for p in key_configs])
key_configs = key_configs.reshape((1, len(key_configs), 4, 1))
key_configs = key_configs.repeat(len(poses), axis=0)
inp = {
'goal_flags': goal_flags,
'key_configs': key_configs,
'collisions': collisions,
'poses': poses
}
return inp
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 get_placements(state, poses, admon, smpler_state):
stime = time.time()
# placement, value = admon.get_max_x(state, poses)
max_x = None
max_val = -np.inf
exp_val = {}
cols = state[:, :, 0:2, :]
goal_flags = state[:, :, 2:, :]
key_configs = pickle.load(open('prm.pkl', 'r'))[0]
key_configs = np.delete(key_configs, [415, 586, 615, 618, 619], axis=0)
rel_konfs = []
for k in key_configs:
konf = utils.clean_pose_data(k)
obj_pose = utils.clean_pose_data(smpler_state.obj_pose)
rel_konf = utils.subtract_pose2_from_pose1(konf, obj_pose)
rel_konfs.append(rel_konf)
rel_konfs = np.array(rel_konfs).reshape((1, 615, 3, 1))
x_range = np.linspace(0., 3.5, 10)
y_range = np.linspace(-8., -6., 10)
placement = np.array([0., 0., 0.])
for x in x_range:
for y in y_range:
placement = placement.squeeze()
placement[0] = x
placement[1] = y
placement = utils.clean_pose_data(placement)
obj_pose = utils.clean_pose_data(smpler_state.obj_pose)
rel_placement = utils.subtract_pose2_from_pose1(
placement, obj_pose)
obj_pose = utils.encode_pose_with_sin_and_cos_angle(obj_pose)
val = admon.q_mse_model.predict([
rel_placement[None, :], goal_flags, obj_pose[None, :],
rel_konfs, cols
])
#print np.mean(admon.relevance_model.predict([rel_placement[None, :], goal_flags, rel_konfs, cols]).squeeze())
if val > max_val:
max_x = copy.deepcopy(placement)
max_val = val
exp_val[(x, y)] = np.exp(val) * 100
print rel_placement, x, y, val, exp_val[(x, y)]
total = np.sum(exp_val.values())
total = 1
i = 0
utils.viewer()
for x in x_range:
for y in y_range:
height = exp_val[(x, y)] # / total + 1
# print x, y, height
placement = placement.squeeze()
placement[0] = x
placement[1] = y
# absx, absy = unnormalize_pose_wrt_region(placement, 'loading_region')[0:2]
# make_body(height, i, absx, absy)
if height == np.exp(max_val) * 100:
make_body(height, i, x, y, color=[1, 0, 0])
else:
make_body(height, i, x, y)
i += 1
placement = max_x
print placement, max_val, np.exp(max_val)
print 'maximizing x time', time.time() - stime
def main():
device = torch.device("cpu")
edges = pickle.load(open('prm_edges_for_reachability_gnn.pkl', 'r'))
n_msg_passing = 0
net = ReachabilityNet(edges,
n_key_configs=618,
device=device,
n_msg_passing=n_msg_passing)
action_type = 'place'
load_weights(net, 35, action_type, 1, n_msg_passing, device)
get_data_test_acc = False
if get_data_test_acc:
seed = 0
np.random.seed(seed)
torch.cuda.manual_seed_all(seed)
dataset = GNNReachabilityDataset(action_type)
n_train = int(len(dataset) * 0.9)
trainset, testset = torch.utils.data.random_split(
dataset, [n_train, len(dataset) - n_train])
testloader = torch.utils.data.DataLoader(dataset,
batch_size=32,
shuffle=False,
num_workers=20,
pin_memory=True)
print "Getting test accuracy for n_test %d..." % len(testset)
test_acc = get_test_acc(testloader, net, device, len(testset))
print test_acc
import pdb
pdb.set_trace()
problem_seed = 1
problem_env, openrave_env = create_environment(problem_seed)
if 'Encoded' in net.__class__.__name__:
tmp, _ = pickle.load(open('prm.pkl', 'r'))
key_configs = np.zeros((618, 4))
for pidx, p in enumerate(tmp):
key_configs[pidx] = utils.encode_pose_with_sin_and_cos_angle(p)
else:
key_configs, _ = pickle.load(open('prm.pkl', 'r'))
compute_clf_acc = False
if compute_clf_acc:
compute_clf_rates_from_diff_pinstances(problem_env, key_configs, net,
action_type)
utils.viewer()
if action_type == 'place':
status = "NoSolution"
while status != "HasSolution":
sampler = UniformSampler(problem_env.regions['home_region'])
checker = TwoArmPaPFeasibilityCheckerWithoutSavingFeasiblePick(
problem_env)
pick_param = sampler.sample()
target_obj = problem_env.objects[np.random.randint(8)]
abstract_action = Operator(
operator_type='two_arm_pick_two_arm_place',
discrete_parameters={
'object': target_obj,
'place_region': 'home_region'
})
op_parameters, status = checker.check_feasibility(
abstract_action, pick_param)
utils.two_arm_pick_object(target_obj, op_parameters['pick'])
collisions = utils.compute_occ_vec(key_configs)
col = utils.convert_binary_vec_to_one_hot(collisions.squeeze())
qg = sample_qg(problem_env, 'home_region')
utils.visualize_path([qg])
vertex = make_vertices(qg, key_configs, col, net)
vertex_outputs = net.get_vertex_activations(vertex).data.numpy().squeeze()
"""
top_activations = np.max(vertex_outputs, axis=0)
top_k_args = np.argsort(abs(top_activations))[-10:]
"""
top_k_args = np.argsort(abs(vertex_outputs))[-30:]
top_k_key_configs = key_configs[top_k_args, :]
import pdb
pdb.set_trace()
utils.visualize_path(top_k_key_configs)
# todo visualize the activations
import pdb
pdb.set_trace()
def get_processed_poses_from_action(state, action):
if action_data_mode == 'absolute':
pick_pose = utils.encode_pose_with_sin_and_cos_angle(
action['pick_abs_base_pose'])
place_pose = utils.encode_pose_with_sin_and_cos_angle(
action['place_abs_base_pose'])
elif action_data_mode == 'pick_relative':
pick_pose = action['pick_abs_base_pose']
pick_pose = utils.get_relative_robot_pose_wrt_body_pose(
pick_pose, state.abs_obj_pose)
pick_pose = utils.encode_pose_with_sin_and_cos_angle(pick_pose)
place_pose = utils.encode_pose_with_sin_and_cos_angle(
action['place_abs_base_pose'])
elif action_data_mode == 'pick_relative_place_relative_to_region':
pick_pose = action['pick_abs_base_pose']
pick_pose = utils.get_relative_robot_pose_wrt_body_pose(
pick_pose, state.abs_obj_pose)
pick_pose = utils.encode_pose_with_sin_and_cos_angle(pick_pose)
place_pose = get_place_pose_wrt_region(action['place_abs_base_pose'],
action['region_name'])
elif action_data_mode == 'pick_parameters_place_relative_to_region':
pick_pose = action['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose)
base_angle = utils.encode_angle_in_sin_and_cos(base_angle)
pick_pose = np.hstack([portion, base_angle, facing_angle_offset])
place_pose = get_place_pose_wrt_region(action['place_abs_base_pose'],
action['region_name'])
place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose)
elif action_data_mode == 'pick_parameters_place_normalized_relative_to_region':
pick_pose = action['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose)
base_angle = utils.encode_angle_in_sin_and_cos(base_angle)
pick_pose = np.hstack([portion, base_angle, facing_angle_offset])
place_pose = normalize_place_pose_wrt_region(
action['place_abs_base_pose'], action['region_name'])
place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose)
elif action_data_mode == 'pick_parameters_place_relative_to_pick':
pick_pose = action['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose)
base_angle = utils.encode_angle_in_sin_and_cos(base_angle)
pick_params = np.hstack([portion, base_angle, facing_angle_offset])
place_pose = action['place_abs_base_pose']
place_pose = utils.get_relative_robot_pose_wrt_body_pose(
place_pose, pick_pose)
pick_pose = pick_params
place_pose = utils.encode_pose_with_sin_and_cos_angle(place_pose)
elif action_data_mode == 'pick_parameters_place_relative_to_object':
pick_pose = action['pick_abs_base_pose']
portion, base_angle, facing_angle_offset \
= utils.get_ir_parameters_from_robot_obj_poses(pick_pose, state.abs_obj_pose)
base_angle = utils.encode_angle_in_sin_and_cos(base_angle)
pick_params = np.hstack([portion, base_angle, facing_angle_offset])
pick_pose = pick_params
place_pose = action['place_abs_base_pose']
obj_pose = state.abs_obj_pose
rel_place_pose = utils.get_relative_robot_pose_wrt_body_pose(
place_pose, obj_pose)
place_pose = utils.encode_pose_with_sin_and_cos_angle(rel_place_pose)
unprocessed_place = utils.clean_pose_data(
get_unprocessed_placement(place_pose, obj_pose))
target = utils.clean_pose_data(action['place_abs_base_pose'])
is_recovered = np.all(np.isclose(unprocessed_place, target))
try:
assert is_recovered
except:
import pdb
pdb.set_trace()
action = np.hstack([pick_pose, place_pose])
return action
