def process_single_data(fname, problem_env, key_configs, save_file):
mp_results = pickle.load(open(fname, 'r'))
first_obj_poses = mp_results[0]['object_poses']
[
utils.set_obj_xytheta(first_obj_poses[obj.GetName()], obj.GetName())
for obj in problem_env.objects
]
pick_q0s = []
pick_qgs = []
pick_labels = []
pick_collisions = []
place_q0s = []
place_qgs = []
place_labels = []
place_collisions = []
collision_vector = utils.compute_occ_vec(key_configs)
for mp_result in mp_results:
object_poses = mp_result['object_poses']
assert object_poses == first_obj_poses
if mp_result['held_obj'] is None:
pick_q0s.append(mp_result['q0'])
pick_qgs.append(mp_result['qg'])
pick_labels.append(mp_result['label'])
pick_collisions.append(collision_vector)
else:
place_q0s.append(mp_result['q0'])
place_qgs.append(mp_result['qg'])
place_labels.append(mp_result['label'])
utils.two_arm_pick_object(mp_result['held_obj'],
{'q_goal': mp_result['q0']})
place_collision = utils.compute_occ_vec(key_configs)
utils.two_arm_place_object({'q_goal': mp_result['q0']})
place_collisions.append(place_collision)
pickle.dump(
{
'pick_q0s': pick_q0s,
'pick_qgs': pick_qgs,
'pick_collisions': pick_collisions,
'pick_labels': pick_labels,
'place_q0s': place_q0s,
'place_qgs': place_qgs,
'place_collisions': place_collisions,
'place_labels': place_labels
}, open(save_file, 'wb'))
print "Done with file", fname
def compute_collision_and_make_predictions(qg, key_configs, net):
collisions = utils.compute_occ_vec(key_configs)
col = utils.convert_binary_vec_to_one_hot(collisions.squeeze())
vertex = make_vertices(qg, key_configs, col, net)
output = net(vertex)
# print output, (output > 0.5).numpy()[0, 0]
# print output
return (output > 0.5).numpy()[0, 0]
def get_collison_vector(self, given_collision_vector):
if given_collision_vector is None:
collision_vector = utils.compute_occ_vec(self.key_configs)
else:
collision_vector = given_collision_vector
collision_vector = utils.convert_binary_vec_to_one_hot(
collision_vector)
collision_vector = collision_vector.reshape(
(1, len(collision_vector), 2, 1))
return collision_vector
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
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()