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 compute_clf_rates_from_diff_pinstances(problem_env, key_configs, net,
action_type):
clf_rates = []
for problem_seed in range(400):
np.random.seed(problem_seed)
random.seed(problem_seed)
[
utils.randomly_place_region(obj,
problem_env.regions['loading_region'])
for obj in problem_env.objects
]
utils.randomly_place_region(problem_env.robot,
problem_env.regions['loading_region'])
if action_type == 'place':
sampler = UniformSampler(problem_env.regions['home_region'])
checker = TwoArmPaPFeasibilityCheckerWithoutSavingFeasiblePick(
problem_env)
status = "NoSolution"
while status != "HasSolution":
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'])
region_name = 'loading_region'
else:
region_name = 'loading_region'
clf_rate = get_clf_accuracy(problem_env, key_configs, net, region_name)
if action_type == 'place':
utils.two_arm_place_object(op_parameters['pick'])
clf_rates.append(clf_rate)
# print "pidx %d Clf rate %.2f " % (problem_seed, clf_rate)
print np.array(clf_rates).sum(axis=0), problem_seed
def get_sampler(config, abstract_state, abstract_action,
learned_sampler_model):
if not config.use_learning:
if 'uniform' in config.sampling_strategy:
target_region = abstract_state.problem_env.regions[
abstract_action.discrete_parameters['place_region']]
if 'two_arm' in config.domain:
sampler = UniformSampler(atype='two_arm_pick_and_place',
target_region=target_region)
else:
sampler = {
'pick':
UniformSampler(target_region=None, atype='one_arm_pick'),
'place':
UniformSampler(target_region=target_region,
atype='one_arm_place')
}
elif 'voo' in config.sampling_strategy:
raise NotImplementedError
else:
raise NotImplementedError
else:
if 'two_arm' in config.domain:
sampler = PickPlaceLearnedSampler('two_arm_pick_and_place',
learned_sampler_model,
abstract_state, abstract_action)
else:
pick_sampler = PickOnlyLearnedSampler('one_arm_pick',
learned_sampler_model,
abstract_state,
abstract_action)
place_sampler = PlaceOnlyLearnedSampler(
'one_arm_place',
learned_sampler_model,
abstract_state,
abstract_action,
smpler_state=pick_sampler.smpler_state)
sampler = {'pick': pick_sampler, 'place': place_sampler}
return sampler
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 get_learned_sampler_models(config):
if not config.use_learning:
return None
if 'two_arm' in config.domain:
train_type = config.train_type
config.atype = 'place'; config.region = 'home_region'; config.seed = config.sampler_seed; config.train_type='wgangp'
goal_region_place_model = make_sampler_model_and_load_weights(config)
config.train_type = train_type
config.atype = 'place'; config.region = 'loading_region'; config.seed = config.sampler_seed
obj_region_place_model = make_sampler_model_and_load_weights(config)
config.atype = 'pick'; config.region = ''; config.seed = config.sampler_seed
pick_model = make_sampler_model_and_load_weights(config)
else:
goal_region_place_model = UniformSampler(target_region='rectangular_packing_box1_region',
atype='one_arm_place') # I don't think we need to learn sampler for this
config.atype = 'place'; config.region = 'center_shelf_region'; config.seed = config.sampler_seed
obj_region_place_model = make_sampler_model_and_load_weights(config)
config.atype = 'pick'; config.region = ''; config.seed = config.sampler_seed
pick_model = make_sampler_model_and_load_weights(config)
model = {'place_goal_region': goal_region_place_model, 'place_obj_region': obj_region_place_model, 'pick': pick_model}
return model
def create_sampling_agent(self, node):
operator_skeleton = node.operator_skeleton
if 'one_arm' in self.problem_env.name:
dont_check_motion_existence = True
else:
dont_check_motion_existence = False
abstract_state = node.state
abstract_action = node.operator_skeleton
place_region = self.problem_env.regions[
abstract_action.discrete_parameters['place_region']]
if self.sampling_strategy == 'uniform':
sampler = UniformSampler(place_region)
generator = TwoArmPaPGenerator(
abstract_state,
abstract_action,
sampler,
n_parameters_to_try_motion_planning=self.n_motion_plan_trials,
n_iter_limit=self.n_feasibility_checks,
problem_env=self.problem_env,
pick_action_mode='ir_parameters',
place_action_mode='object_pose')
elif self.sampling_strategy == 'voo':
target_obj = abstract_action.discrete_parameters['object']
sampler = VOOSampler(
target_obj, place_region, self.explr_p,
self.problem_env.reward_function.worst_potential_value)
generator = TwoArmVOOGenerator(
abstract_state,
abstract_action,
sampler,
n_parameters_to_try_motion_planning=self.n_motion_plan_trials,
n_iter_limit=self.n_feasibility_checks,
problem_env=self.problem_env,
pick_action_mode='ir_parameters',
place_action_mode='object_pose')
else:
raise NotImplementedError
return generator
def fcn(o, r, s):
global num_ik_checks
global num_mp_checks
if problem.name == 'two_arm_mover':
abstract_state = None # TODO: figure out what goes here
abstract_action = Operator(
'two_arm_pick_two_arm_place', {
'object': problem.env.GetKinBody(o),
'place_region': problem.regions[r]
})
sampler = UniformSampler(problem.regions[r])
generator = TwoArmPaPGenerator(
abstract_state,
abstract_action,
sampler,
n_parameters_to_try_motion_planning=config.n_mp_limit,
n_iter_limit=config.n_iter_limit,
problem_env=problem,
pick_action_mode='ir_parameters',
place_action_mode='object_pose')
while True:
s.Restore()
prev_ik_checks = generator.n_ik_checks
prev_mp_checks = generator.n_mp_checks
params = generator.sample_next_point()
num_ik_checks += generator.n_ik_checks - prev_ik_checks
num_mp_checks += generator.n_mp_checks - prev_mp_checks
if params['is_feasible']:
abstract_action.continuous_parameters = params
abstract_action.execute()
t = CustomStateSaver(problem.env)
yield params, t
else:
yield None
elif problem.name == 'one_arm_mover':
while True:
s.Restore()
action = Operator(
'one_arm_pick_one_arm_place', {
'object': problem.env.GetKinBody(o),
'place_region': problem.regions[r]
})
current_region = problem.get_region_containing(
problem.env.GetKinBody(o)).name
sampler = OneArmPaPUniformGenerator(
action,
problem,
cached_picks=(iksolutions[current_region], iksolutions[r]))
pick_params, place_params, status = sampler.sample_next_point(
max_ik_attempts=500)
if status == 'HasSolution':
action.continuous_parameters = {
'pick': pick_params,
'place': place_params
}
action.execute()
t = CustomStateSaver(problem.env)
yield action.continuous_parameters, t
else:
yield None
else:
raise NotImplementedError
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()