def compute_state(self, obj, region):
# todo the state of a concrete node consists of the object, region, and the collision vector.
if 'two_arm_mover' in self.problem_env.name:
goal_entities = ['square_packing_box1', 'home_region']
else:
raise NotImplementedError
return ConcreteNodeState(self.problem_env, obj, region, goal_entities)
def __init__(self,
operator_skeleton,
problem_env,
sampler,
abstract_state,
max_n_iter,
swept_volume_constraint=None):
PaPUniformGenerator.__init__(self, operator_skeleton, problem_env,
max_n_iter, swept_volume_constraint)
self.feasible_pick_params = {}
self.sampler = sampler
self.abstract_state = abstract_state
self.obj = operator_skeleton.discrete_parameters['object']
self.region = operator_skeleton.discrete_parameters['region']
goal_entities = self.abstract_state.goal_entities
self.smpler_state = ConcreteNodeState(
self.problem_env,
self.obj,
self.region,
goal_entities,
collision_vector=abstract_state.key_config_obstacles)
self.noises_used = []
self.tried_smpls = []
# to do generate 1000 smpls here
n_total_iters = sum(range(10, self.max_n_iter, 10))
# generate n_total_iters number of samples - can I be systematic about this, instead of random smpling?
# I guess they will have to live in the same space; but I need to increase the variance
"""
z_smpl_fname = 'z_smpls.pkl'
if os.path.isfile(z_smpl_fname):
z_smpls = pickle.load(open(z_smpl_fname, 'r'))
else:
z_smpls = []
i = 0
for _ in range(n_total_iters): # even pre-store these
if 0 < len(z_smpls) < 50:
min_dist = np.min(np.linalg.norm(new_z - np.array(z_smpls), axis=-1))
while min_dist < 1:
new_z = i * np.random.normal(size=(1, 4)).astype('float32')
min_dist = np.min(np.linalg.norm(new_z - np.array(z_smpls), axis=-1))
i += 1
else:
new_z = np.random.normal(size=(1, 4)).astype('float32')
z_smpls.append(new_z)
pickle.dump(z_smpls, open(z_smpl_fname, 'wb'))
"""
z_smpls = noise(z_size=(1900, 4))
z_smpls = np.vstack([np.array([0, 0, 0, 0]), z_smpls])
self.policy_smpl_batch = generate_policy_smpl_batch(
self.smpler_state, self.sampler, z_smpls)
self.policy_smpl_idx = 0
def __init__(self, sampler, abstract_state, abstract_action):
Sampler.__init__(self, sampler)
self.key_configs = abstract_state.prm_vertices
self.abstract_state = abstract_state
self.obj = abstract_action.discrete_parameters['object']
self.region = abstract_action.discrete_parameters['place_region']
goal_entities = self.abstract_state.goal_entities
stime = time.time()
self.smpler_state = ConcreteNodeState(abstract_state.problem_env, self.obj, self.region, goal_entities, key_configs=self.key_configs)
print "Concre node creation time", time.time()-stime
self.samples = self.sample_new_points(100)
utils.viewer()
self.curr_smpl_idx = 0
def compute_state(obj, region, problem_env):
goal_entities = [
'square_packing_box1', 'square_packing_box2', 'square_packing_box3',
'square_packing_box4', 'home_region'
]
goal_entities = [
'rectangular_packing_box1', 'rectangular_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region'
]
#goal_entities = ['square_packing_box1', 'square_packing_box2',
# 'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region']
goal_entities = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4', 'home_region'
]
return ConcreteNodeState(problem_env, obj, region, goal_entities)
def __init__(self, policy, abstract_state, abstract_action):
Sampler.__init__(self, policy)
self.key_configs = abstract_state.prm_vertices
self.abstract_state = abstract_state
self.obj = abstract_action.discrete_parameters['object']
self.region = abstract_action.discrete_parameters['place_region']
goal_entities = self.abstract_state.goal_entities
self.smpler_state = ConcreteNodeState(abstract_state.problem_env, self.obj, self.region,
goal_entities,
key_configs=self.key_configs)
self.noises_used = []
self.tried_smpls = []
z_smpls = noise(z_size=(101, 7))
smpls = generate_pick_and_place_batch(self.smpler_state, self.policy, z_smpls)
self.policy_smpl_batch = unprocess_pick_and_place_smpls(smpls)
self.policy_smpl_idx = 0
def compute_state(obj, region, problem_env, key_configs):
# todo the state of a concrete node consists of the object, region, and the collision vector.
goal_entities = ['square_packing_box1', 'home_region']
return ConcreteNodeState(problem_env, obj, region, goal_entities,
key_configs)
def compute_state(obj, region, problem_env):
goal_entities = ['square_packing_box1', 'home_region']
return ConcreteNodeState(problem_env, obj, region, goal_entities)
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