def is_collision_and_region_constraints_satisfied(self, target_robot_region1, target_robot_region2,
target_obj_region):
target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \
target_robot_region2.contains(self.robot.ComputeAABB())
obj = self.robot.GetGrabbed()[0]
if not target_region_contains:
return False
if not target_obj_region.contains(obj.ComputeAABB()):
return False
is_object_pose_infeasible = self.env.CheckCollision(obj)
if is_object_pose_infeasible:
return False
is_base_pose_infeasible = self.env.CheckCollision(self.robot)
if is_base_pose_infeasible:
return False
else:
dof_vals = self.robot.GetDOFValues()
utils.release_obj()
utils.fold_arms()
self.problem_env.set_robot_to_default_dof_values()
is_base_pose_infeasible = self.env.CheckCollision(self.robot)
self.robot.SetDOFValues(dof_vals)
utils.grab_obj(obj)
if is_base_pose_infeasible:
return False
return True
def restore(self, state_saver):
curr_obj = state_saver.curr_obj
which_operator = state_saver.which_operator
if not which_operator == 'two_arm_pick':
grab_obj(self.robot, curr_obj)
else:
if len(self.robot.GetGrabbed()) > 0:
release_obj(self.robot, self.robot.GetGrabbed()[0])
state_saver.Restore()
self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y
| DOFAffine.RotationAxis, [0, 0, 1])
def place_object_and_robot_at_new_pose(self, obj, obj_pose, obj_region):
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()), self.robot.GetTransform())
if len(self.robot.GetGrabbed()) > 0:
release_obj()
set_obj_xytheta(obj_pose, obj)
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
self.robot.SetTransform(new_T_robot)
new_base_pose = get_body_xytheta(self.robot)
set_robot_config(new_base_pose, self.robot)
fold_arms()
set_point(obj, np.hstack([obj_pose[0:2], obj_region.z + 0.001]))
return new_base_pose
def compute_robot_base_pose_given_object_pose(obj, robot, obj_pose,
T_r_wrt_o):
original_robot_T = robot.GetTransform()
release_obj(robot, obj)
set_obj_xytheta(obj_pose, obj)
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
robot.SetTransform(new_T_robot)
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis,
[0, 0, 1])
robot_xytheta = robot.GetActiveDOFValues()
grab_obj(robot, obj)
robot.SetTransform(original_robot_T)
return robot_xytheta
def get_objects_in_collision_with_given_op_inst(self, op_inst):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
fold_arms()
new_cols = self.problem_env.get_objs_in_collision(op_inst.low_level_motion, 'entire_region')
saver.Restore()
if held is not None:
grab_obj(held)
return new_cols
def is_swept_volume_cleared(self, obj):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
collision_occurred = self.pick_swept_volume.is_collision_in_all_volumes(obj) \
or self.place_swept_volume.is_collision_in_all_volumes(obj)
saver.Restore()
if held is not None:
grab_obj(held)
if collision_occurred:
return False
else:
return True
def check_collision_in_pap(self, pick_op, place_op, obj_object, swept_volume):
old_tf = obj_object.GetTransform()
collision = False
pick_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
utils.release_obj()
obj_object.SetTransform(old_tf)
return True
place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot):
obj_object.SetTransform(old_tf)
return True
if self.problem_env.env.CheckCollision(obj_object):
obj_object.SetTransform(old_tf)
return True
is_object_pose_infeasible = not swept_volume.is_swept_volume_cleared(obj_object)
if is_object_pose_infeasible:
obj_object.SetTransform(old_tf)
return True
obj_object.SetTransform(old_tf)
return collision
def get_placement(self, obj, target_obj_region, T_r_wrt_o):
original_trans = self.robot.GetTransform()
original_config = self.robot.GetDOFValues()
self.robot.SetTransform(original_trans)
self.robot.SetDOFValues(original_config)
release_obj(self.robot, obj)
with self.robot:
# print target_obj_region
obj_pose = randomly_place_in_region(
self.env, obj, target_obj_region) # randomly place obj
obj_pose = obj_pose.squeeze()
# compute the resulting robot transform
new_T_robot = np.dot(obj.GetTransform(), T_r_wrt_o)
self.robot.SetTransform(new_T_robot)
self.robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y
| DOFAffine.RotationAxis, [0, 0, 1])
robot_xytheta = self.robot.GetActiveDOFValues()
set_robot_config(robot_xytheta, self.robot)
grab_obj(self.robot, obj)
return obj_pose, robot_xytheta
def sample_from_discretized_set(self):
(pick_tf, pick_params), (place_tf, place_params) = random.choice(zip(*self.cached_picks))
pick_region = self.problem_env.get_region_containing(self.target_obj)
place_region = self.place_op.discrete_parameters['place_region']
pick_params = copy.deepcopy(pick_params)
place_params = copy.deepcopy(place_params)
old_tf = self.target_obj.GetTransform()
old_pose = get_body_xytheta(self.target_obj).squeeze()
self.pick_op.continuous_parameters = place_params
self.target_obj.SetTransform(place_tf)
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
# This is the only sampling here
place_pose = self.place_generator.sample_from_uniform()
place_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj,
place_pose,
place_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(
self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
if not place_region.contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
place_params['operator_name'] = 'one_arm_place'
place_params['object_pose'] = place_pose
place_params['action_parameters'] = place_pose
place_params['base_pose'] = place_base_pose
place_params['q_goal'][-3:] = place_base_pose
self.place_op.continuous_parameters = place_params
self.pick_op.continuous_parameters = pick_params # is reference and will be changed lader
self.target_obj.SetTransform(pick_tf)
# assert_region(pick_region)
# self.pick_op.execute()
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
pick_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj,
old_pose, pick_region)
pick_params['q_goal'][-3:] = pick_base_pose
# assert_region(pick_region)
# self.target_obj.SetTransform(pick_tf)
# self.pick_op.execute()
# tf = np.dot(np.dot(old_tf, np.linalg.pinv(pick_tf)), self.problem_env.robot.GetTransform())
# self.place_op.execute()
# self.target_obj.SetTransform(old_tf)
# self.problem_env.robot.SetTransform(tf)
# pick_base_pose = get_body_xytheta(self.problem_env.robot).squeeze()
# pick_params['q_goal'][-3:] = pick_base_pose
bad = False
self.target_obj.SetTransform(old_tf)
self.pick_op.execute()
# assert_region(pick_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot):
release_obj()
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
self.place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(
self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
if not self.place_op.discrete_parameters['place_region'].contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
self.target_obj.SetTransform(old_tf)
return pick_params, place_params, 'HasSolution'
def check_feasibility(self,
operator_skeleton,
place_parameters,
swept_volume_to_avoid=None):
# Note:
# this function checks if the target region contains the robot when we place object at place_parameters
# and whether the robot will be in collision
obj = self.robot.GetGrabbed()[0]
obj_region = operator_skeleton.discrete_parameters['region']
if type(obj_region) == str:
obj_region = self.problem_env.regions[obj_region]
obj_pose = place_parameters
T_r_wrt_o = np.dot(np.linalg.inv(obj.GetTransform()),
self.robot.GetTransform())
original_trans = self.robot.GetTransform()
original_obj_trans = obj.GetTransform()
target_robot_region1 = self.problem_env.regions['home_region']
target_robot_region2 = self.problem_env.regions['loading_region']
target_obj_region = obj_region # for fetching, you want to move it around
robot_xytheta = self.compute_robot_base_pose_given_object_pose(
obj, self.robot, obj_pose, T_r_wrt_o)
set_robot_config(robot_xytheta, self.robot)
# why do I disable objects in region? Most likely this is for minimum constraint computation
#self.problem_env.disable_objects_in_region('entire_region')
target_region_contains = target_robot_region1.contains(self.robot.ComputeAABB()) or \
target_robot_region2.contains(self.robot.ComputeAABB())
is_base_pose_infeasible = self.env.CheckCollision(self.robot) or \
(not target_region_contains)
is_object_pose_infeasible = self.env.CheckCollision(obj) or \
(not target_obj_region.contains(obj.ComputeAABB()))
#self.problem_env.enable_objects_in_region('entire_region')
if is_base_pose_infeasible or is_object_pose_infeasible:
action = {
'operator_name': 'two_arm_place',
'base_pose': None,
'object_pose': None,
'action_parameters': place_parameters
}
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
return action, 'NoSolution'
else:
release_obj(self.robot, obj)
if swept_volume_to_avoid is not None:
# release the object
if not swept_volume_to_avoid.is_swept_volume_cleared(obj):
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {
'operator_name': 'two_arm_place',
'base_pose': None,
'object_pose': None,
'action_parameters': place_parameters
}
return action, 'NoSolution'
"""
for config in swept_volume_to_avoid:
set_robot_config(config, self.robot)
if self.env.CheckCollision(self.robot, obj):
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {'operator_name': 'two_arm_place', 'base_pose': None, 'object_pose': None,
'action_parameters': place_parameters}
return action, 'NoSolution'
"""
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(self.robot, obj)
action = {
'operator_name': 'two_arm_place',
'base_pose': robot_xytheta,
'object_pose': obj_pose,
'action_parameters': place_parameters
}
return action, 'HasSolution'
def main():
problem_idx = 0
env_name = 'two_arm_mover'
if env_name == 'one_arm_mover':
problem_env = PaPOneArmMoverEnv(problem_idx)
goal = ['rectangular_packing_box1_region'
] + [obj.GetName() for obj in problem_env.objects[:3]]
state_fname = 'one_arm_state_gpredicate.pkl'
else:
problem_env = PaPMoverEnv(problem_idx)
goal_objs = [
'square_packing_box1', 'square_packing_box2',
'rectangular_packing_box3', 'rectangular_packing_box4'
]
goal_region = 'home_region'
goal = [goal_region] + goal_objs
state_fname = 'two_arm_state_gpredicate.pkl'
problem_env.set_goal(goal)
if os.path.isfile(state_fname):
state = pickle.load(open(state_fname, 'r'))
else:
statecls = helper.get_state_class(env_name)
state = statecls(problem_env, problem_env.goal)
utils.viewer()
if env_name == 'one_arm_mover':
obj_name = 'c_obst9'
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={
'object': problem_env.env.GetKinBody(obj_name)
},
continuous_parameters=state.pick_params[obj_name][0])
pick_op.execute()
problem_env.env.Remove(problem_env.env.GetKinBody('computer_table'))
utils.set_color(obj_name, [0.9, 0.8, 0.0])
utils.set_color('c_obst0', [0, 0, 0.8])
utils.set_obj_xytheta(
np.array([[4.47789478, -0.01477591, 4.76236795]]), 'c_obst0')
T_viewer = np.array(
[[-0.69618481, -0.41674492, 0.58450867, 3.62774134],
[-0.71319601, 0.30884202, -0.62925993, 0.39102399],
[0.08172004, -0.85495045, -0.51223194, 1.70261502],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
utils.release_obj()
else:
T_viewer = np.array(
[[0.99964468, -0.00577897, 0.02602139, 1.66357124],
[-0.01521307, -0.92529857, 0.37893419, -7.65383244],
[0.02188771, -0.37919541, -0.92505771, 6.7393589],
[0., 0., 0., 1.]])
viewer = problem_env.env.GetViewer()
viewer.SetCamera(T_viewer)
import pdb
pdb.set_trace()
# prefree and occludespre
target = 'rectangular_packing_box4'
utils.set_obj_xytheta(np.array([[0.1098148, -6.33305931, 0.22135689]]),
target)
utils.get_body_xytheta(target)
utils.visualize_path(
state.cached_pick_paths['rectangular_packing_box4'])
import pdb
pdb.set_trace()
# manipfree and occludesmanip
pick_obj = 'square_packing_box2'
pick_used = state.pick_used[pick_obj]
utils.two_arm_pick_object(pick_obj, pick_used.continuous_parameters)
utils.visualize_path(state.cached_place_paths[(u'square_packing_box2',
'home_region')])
import pdb
pdb.set_trace()
def sample_cont_params(self):
# todo refactor this function
robot_pose = utils.get_body_xytheta(self.robot)
robot_config = self.robot.GetDOFValues()
assert len(self.robot.GetGrabbed()) == 0
if self.cached_picks is not None:
(pick_tf, pick_params), (place_tf, place_params) = random.choice(zip(*self.cached_picks))
pick_region = self.problem_env.get_region_containing(self.target_obj)
place_region = self.place_op.discrete_parameters['region']
pick_params = copy.deepcopy(pick_params)
place_params = copy.deepcopy(place_params)
#assert pick_params['operator_name'] == 'one_arm_pick'
#assert place_params['operator_name'] == 'one_arm_pick'
old_tf = self.target_obj.GetTransform()
old_pose = get_body_xytheta(self.target_obj).squeeze()
self.pick_op.continuous_parameters = place_params
self.target_obj.SetTransform(place_tf)
#self.pick_op.execute()
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
place_pose = self.place_generator.sample_from_uniform()
place_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj, place_pose, place_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
def assert_region(region):
try:
assert self.problem_env.get_region_containing(self.target_obj).name == region.name
return True
except Exception as e:
print(e)
set_color(self.target_obj, [1,1,1])
import pdb
pdb.set_trace()
return False
if not place_region.contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
#assert_region(place_region)
place_params['operator_name'] = 'one_arm_place'
place_params['object_pose'] = place_pose
place_params['action_parameters'] = place_pose
place_params['base_pose'] = place_base_pose
place_params['q_goal'][-3:] = place_base_pose
self.place_op.continuous_parameters = place_params
self.pick_op.continuous_parameters = pick_params # is reference and will be changed lader
self.target_obj.SetTransform(pick_tf)
#assert_region(pick_region)
#self.pick_op.execute()
set_robot_config(self.pick_op.continuous_parameters['q_goal'][-3:])
pick_base_pose = self.place_feasibility_checker.place_object_and_robot_at_new_pose(self.target_obj, old_pose, pick_region)
pick_params['q_goal'][-3:] = pick_base_pose
#assert_region(pick_region)
#self.target_obj.SetTransform(pick_tf)
#self.pick_op.execute()
#tf = np.dot(np.dot(old_tf, np.linalg.pinv(pick_tf)), self.problem_env.robot.GetTransform())
#self.place_op.execute()
#self.target_obj.SetTransform(old_tf)
#self.problem_env.robot.SetTransform(tf)
#pick_base_pose = get_body_xytheta(self.problem_env.robot).squeeze()
#pick_params['q_goal'][-3:] = pick_base_pose
bad = False
self.target_obj.SetTransform(old_tf)
self.pick_op.execute()
#assert_region(pick_region)
if self.problem_env.env.CheckCollision(self.problem_env.robot):
release_obj()
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
self.place_op.execute()
if self.problem_env.env.CheckCollision(self.problem_env.robot) or self.problem_env.env.CheckCollision(self.target_obj):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
if not self.place_op.discrete_parameters['region'].contains(self.target_obj.ComputeAABB()):
self.target_obj.SetTransform(old_tf)
return None, None, 'InfeasibleIK'
#assert_region(place_region)
#place_action = {
# 'operator_name': 'one_arm_place',
# 'q_goal': np.hstack([grasp_config, new_base_pose]),
# 'base_pose': new_base_pose,
# 'object_pose': place_parameters,
# 'action_parameters': obj_pose,
# 'g_config': grasp_config,
# 'grasp_params': grasp_params,
#}
#pick_action = {
# 'operator_name': operator_skeleton.type,
# 'q_goal': np.hstack([grasp_config, pick_base_pose]),
# 'grasp_params': grasp_params,
# 'g_config': grasp_config,
# 'action_parameters': pick_parameters,
#}
self.target_obj.SetTransform(old_tf)
#assert pick_params['operator_name'] == 'one_arm_pick'
#assert place_params['operator_name'] == 'one_arm_place'
return pick_params, place_params, 'HasSolution'
# sample pick
pick_cont_params = None
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
while pick_cont_params is None:
pick_cont_params, status = self.sample_pick_cont_parameters()
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 500:
break
if status != 'HasSolution':
utils.set_robot_config(robot_pose)
return None, None, status
self.pick_op.continuous_parameters = pick_cont_params
self.pick_op.execute()
# sample place
n_ik_attempts = 0
n_base_attempts = 0
status = "NoSolution"
place_cont_params = None
while place_cont_params is None:
place_cont_params, status = self.sample_place_cont_parameters(pick_cont_params)
if status == 'InfeasibleBase':
n_base_attempts += 1
elif status == 'InfeasibleIK':
n_ik_attempts += 1
elif status == 'HasSolution':
n_ik_attempts += 1
break
if n_ik_attempts == 1 or n_base_attempts == 500:
break
utils.one_arm_place_object(pick_cont_params)
self.robot.SetDOFValues(robot_config)
utils.set_robot_config(robot_pose)
if status != 'HasSolution':
return None, None, status
else:
self.place_op.continuous_parameters = place_cont_params
return pick_cont_params, place_cont_params, status
def main():
config = parse_arguments()
solution_file_name = get_solution_file_name(config)
is_problem_solved_before = os.path.isfile(solution_file_name)
if config.gather_planning_exp:
assert config.h_option == 'hcount_old_number_in_goal'
#config.timelimit =
goal_objs, goal_region = get_goal_obj_and_region(config)
print "Goal:", goal_objs, goal_region
problem_env = get_problem_env(config, goal_region, goal_objs)
set_problem_env_config(problem_env, config)
if config.v:
problem_env.env.SetViewer('qtcoin')
if is_problem_solved_before and not config.f:
print "***************Already solved********************"
with open(solution_file_name, 'rb') as f:
data = pickle.load(f)
success = data['success']
tottime = data['tottime']
num_nodes = data['num_nodes']
plan_length = len(data['plan']) if success else 0
print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d N_feasible: %d' % (
tottime, success, plan_length, num_nodes, data['n_feasibility_checks']['ik'])
else:
is_use_gnn = 'qlearned' in config.h_option
if is_use_gnn:
pap_model = get_pap_gnn_model(problem_env, config)
else:
pap_model = None
t = time.time()
np.random.seed(config.planner_seed)
random.seed(config.planner_seed)
learned_sampler_model = get_learned_sampler_models(config)
nodes_to_goal, plan, num_nodes, nodes = search(problem_env, config, pap_model, goal_objs,
goal_region, learned_sampler_model)
tottime = time.time() - t
n_feasibility_checks = get_total_n_feasibility_checks(nodes)
success = plan is not None
plan_length = len(plan) if success else 0
if success and config.domain == 'one_arm_mover':
make_pklable(plan)
if config.gather_planning_exp:
[make_node_pklable(nd) for nd in nodes]
else:
nodes = None
h_for_sampler_training = []
if success:
h_for_sampler_training = []
for n in nodes_to_goal:
h_for_sampler_training.append(n.h_for_sampler_training)
data = {
'n_objs_pack': config.n_objs_pack,
'tottime': tottime,
'success': success,
'plan_length': plan_length,
'num_nodes': num_nodes,
'plan': plan,
'nodes': nodes,
'hvalues': h_for_sampler_training,
'n_feasibility_checks': n_feasibility_checks
}
with open(solution_file_name, 'wb') as f:
pickle.dump(data, f)
print 'Time: %.2f Success: %d Plan length: %d Num nodes: %d' % (tottime, success, plan_length, num_nodes)
print(data['n_feasibility_checks'])
problem_env.reset_to_init_state_stripstream()
color = get_color_of(problem_env.objects[0])
for obj in problem_env.objects:
utils.set_color(obj, [0.0, 0.7, 0.0])
raw_input('continue?')
for op in data['plan']:
o = op.discrete_parameters['object']
obj = problem_env.env.GetKinBody(o)
utils.set_color(obj, [0.8, 0, 0])
raw_input('continue?')
utils.set_robot_config([1000,1000,0])
pick_skeleton = Operator('two_arm_pick', {'object': o})
generator = UniformGenerator(pick_skeleton, problem_env, None)
for i in range(20):
param = generator.sample_next_point(pick_skeleton,
n_iter=10,
n_parameters_to_try_motion_planning=1,
dont_check_motion_existence=True)
if param['is_feasible']:
utils.draw_robot_at_conf(param['q_goal'], .5, 'pick' + str(i), problem_env.robot, problem_env.env)
raw_input('continue?')
utils.remove_drawn_configs('pick')
utils.visualize_path(op.continuous_parameters['pick']['motion'])
# visualize_path calls raw_input and remove_drawn_configs
op.execute_pick()
raw_input('continue?')
utils.set_color(obj, color)
utils.release_obj()
op.execute()
raw_input('continue?')