def compute_place_reward(self, operator_instance):
# todo I can potentially save time by keeping the reward in the node
assert len(self.robot.GetGrabbed()) == 1
prev_robot_config = get_body_xytheta(self.robot)
prev_objects_not_in_goal = self.objects_currently_not_in_goal
object_held = self.robot.GetGrabbed()[0]
two_arm_place_object(object_held, self.robot, operator_instance.continuous_parameters)
new_objects_not_in_goal = self.get_objs_in_collision(self.swept_volume,
'entire_region') # takes about 0.0284 seconds
new_config = get_body_xytheta(self.robot)
#distance_travelled = se2_distance(prev_robot_config, new_config, 1, 1)
if len(prev_objects_not_in_goal) - len(new_objects_not_in_goal) > 0:
distance_travelled = get_trajectory_length(operator_instance.low_level_motion) # 0.3 ms
reward = min(1.0 / distance_travelled, 2)
#reward = 2*np.exp(-distance_travelled)
else:
distance_travelled = get_trajectory_length(operator_instance.low_level_motion)
reward = max(-distance_travelled, self.infeasible_reward)
#reward = max(-1+np.exp(-distance_travelled), -2)
return reward, new_objects_not_in_goal
def check_place_at_base_pose_feasible(self, obj_region, place_base_pose,
swept_volume_to_avoid):
if type(obj_region) == str:
obj_region = self.problem_env.regions[obj_region]
target_obj_region = obj_region # for fetching, you want to move it around
target_robot_region1 = self.problem_env.regions['home_region']
target_robot_region2 = self.problem_env.regions['loading_region']
set_robot_config(place_base_pose, self.robot)
is_feasible = self.is_collision_and_region_constraints_satisfied(
target_robot_region1, target_robot_region2, target_obj_region)
obj = self.robot.GetGrabbed()[0]
original_trans = self.robot.GetTransform()
original_obj_trans = obj.GetTransform()
if not is_feasible:
action = {
'operator_name': 'two_arm_place',
'q_goal': None,
'object_pose': None,
'action_parameters': place_base_pose
}
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
return action, 'NoSolution'
else:
release_obj()
obj_pose = get_body_xytheta(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(obj)
action = {
'operator_name': 'two_arm_place',
'q_goal': None,
'object_pose': None,
'action_parameters': place_base_pose
}
return action, 'NoSolution'
self.robot.SetTransform(original_trans)
obj.SetTransform(original_obj_trans)
grab_obj(obj)
action = {
'operator_name': 'two_arm_place',
'q_goal': place_base_pose,
'object_pose': obj_pose,
'action_parameters': place_base_pose
}
return action, 'HasSolution'
def check_place_feasible(self, pick_parameters, place_parameters,
operator_skeleton, parameter_mode):
pick_op = Operator('two_arm_pick',
operator_skeleton.discrete_parameters)
pick_op.continuous_parameters = pick_parameters
# todo remove the CustomStateSaver
# saver = utils.CustomStateSaver(self.problem_env.env)
original_config = utils.get_body_xytheta(
self.problem_env.robot).squeeze()
pick_op.execute()
place_op = Operator('two_arm_place',
operator_skeleton.discrete_parameters)
place_cont_params, place_status = TwoArmPlaceFeasibilityChecker.check_feasibility(
self, place_op, place_parameters, parameter_mode=parameter_mode)
utils.two_arm_place_object(pick_op.continuous_parameters)
utils.set_robot_config(original_config)
# saver.Restore()
return place_cont_params, place_status
def node_features(entity, mover, goal, is_reachable, blocks_key_configs): isobj = entity not in mover.regions obj = mover.env.GetKinBody(entity) if isobj else None pose = get_body_xytheta(obj)[0] if isobj else None return [ 0, # l 0, # w 0, # h pose[0] if isobj else 0, # x pose[1] if isobj else 0, # y pose[2] if isobj else 0, # theta entity not in mover.regions, # IsObj entity in mover.regions, # IsRoom entity in goal, # IsGoal is_reachable(entity, goal), blocks_key_configs(entity, goal), ]
def compute_g_config(self, obj, pick_base_pose, grasp_params):
original_config = get_body_xytheta(self.robot)
#if True:
with self.robot:
g_config = self.compute_grasp_config(obj, pick_base_pose,
grasp_params)
if g_config is not None:
pick_action = {
'operator_name': 'two_arm_pick',
'base_pose': pick_base_pose,
'grasp_params': grasp_params,
'g_config': g_config
}
two_arm_pick_object(obj, self.robot, pick_action)
if self.problem_env.name == 'convbelt':
feasible = True
else:
inside_region = self.problem_env.regions[
'entire_region'].contains(self.robot.ComputeAABB())
pick_config_not_in_collision = not self.env.CheckCollision(
self.robot)
feasible = inside_region and pick_config_not_in_collision
not_in_collision = not self.env.CheckCollision(self.robot)
feasible = inside_region and not_in_collision
if feasible:
two_arm_place_object(obj, self.robot, pick_action)
set_robot_config(original_config, self.robot)
return g_config
else:
two_arm_place_object(obj, self.robot, pick_action)
set_robot_config(original_config, self.robot)
else:
#if obj.GetName().find("1") != -1:
# import pdb;pdb.set_trace()
set_robot_config(original_config, self.robot)
return None
def check_reachability_precondition(self, operator_instance):
if self.problem_idx == 0:
if operator_instance.type == 'two_arm_place':
held = self.robot.GetGrabbed()[0]
prev_config = get_body_xytheta(self.robot)
set_robot_config(operator_instance.continuous_parameters['base_pose'], self.robot)
if self.regions['forbidden_region'].contains(held.ComputeAABB()) \
or not(self.regions['home_region'].contains(held.ComputeAABB())):
set_robot_config(prev_config, self.robot)
return None, "NoSolution"
set_robot_config(prev_config, self.robot)
motion_planning_region_name = 'home_region'
goal_robot_xytheta = operator_instance.continuous_parameters['base_pose']
if operator_instance.low_level_motion is not None:
motion = operator_instance.low_level_motion
status = 'HasSolution'
return motion, status
motion, status = self.get_base_motion_plan(goal_robot_xytheta, motion_planning_region_name)
return motion, status
def get_problem(mover, goal_objects, goal_region_name, config):
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
constant_map = {}
stream_map = {
'gen-pap': from_gen_fn(gen_pap(mover, config)),
}
obj_names = [obj.GetName() for obj in mover.objects]
obj_poses = [
get_body_xytheta(mover.env.GetKinBody(obj_name)).squeeze()
for obj_name in obj_names
]
initial_robot_conf = get_body_xytheta(mover.robot).squeeze()
if mover.name == 'two_arm_mover':
goal_region = 'home_region'
nongoal_regions = ['loading_region']
elif mover.name == 'one_arm_mover':
goal_region = mover.target_box_region.name
nongoal_regions = ['center_shelf_region'] #list(mover.shelf_regions)
else:
raise NotImplementedError
print(goal_region, nongoal_regions, mover.regions.keys())
init = [('Pickable', obj_name) for obj_name in obj_names]
init += [('InRegion', obj_name,
mover.get_region_containing(mover.env.GetKinBody(obj_name)).name)
for obj_name in obj_names]
init += [('Region', region) for region in nongoal_regions + [goal_region]]
init += [('GoalObject', obj_name) for obj_name in goal_objects]
init += [('NonGoalRegion', region) for region in nongoal_regions]
init_state = CustomStateSaver(mover.env)
init += [('State', init_state)]
init += [('AtState', init_state)]
# robot initialization
init += [('EmptyArm', )]
init += [('AtConf', initial_robot_conf)]
init += [('BaseConf', initial_robot_conf)]
# object initialization
init += [('Pose', obj_pose)
for obj_name, obj_pose in zip(obj_names, obj_poses)]
init += [('PoseInRegion', obj_pose, 'loading_region')
for obj_name, obj_pose in zip(obj_names, obj_poses)]
init += [('AtPose', obj_name, obj_pose)
for obj_name, obj_pose in zip(obj_names, obj_poses)]
goal = ['and'] + [('InRegion', obj_name, goal_region_name)
for obj_name in goal_objects]
print('Num init:', Counter(fact[0] for fact in init))
print('Goal:', goal)
print('Streams:', sorted(stream_map))
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_state(self):
obj_poses = np.array(
[get_body_xytheta(o) for o in self.initial_collisions])
obj_poses = obj_poses.reshape((1, 7 * 3))
return obj_poses
def get_state(self):
# return the poses of objects
obj_poses = np.array([get_body_xytheta(o) for o in self.objects])
obj_poses = obj_poses.reshape((1, 20 * 3))
return obj_poses
def main():
parameters = parse_parameters()
save_dir = make_and_get_save_dir(parameters)
file_path = save_dir + '/seed_' + str(parameters.planner_seed) + '_pidx_' + str(parameters.pidx) + '.pkl'
quit_if_already_tested(file_path, parameters.f)
# for creating problem
np.random.seed(parameters.pidx)
random.seed(parameters.pidx)
is_two_arm_env = parameters.domain.find('two_arm') != -1
if is_two_arm_env:
environment = Mover(parameters.pidx)
else:
environment = OneArmMover(parameters.pidx)
environment.initial_robot_base_pose = get_body_xytheta(environment.robot)
if parameters.domain == 'two_arm_mover':
goal_region = 'home_region'
if parameters.n_objs_pack == 4:
goal_object_names = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3',
'rectangular_packing_box4']
else:
goal_object_names = ['square_packing_box1']
environment.set_goal(goal_object_names, goal_region)
elif parameters.domain == 'one_arm_mover':
goal_region = 'rectangular_packing_box1_region'
assert parameters.n_objs_pack == 1
goal_object_names = ['c_obst1']
environment.set_goal(goal_object_names, goal_region)
goal_entities = goal_object_names + [goal_region]
# for randomized algorithms
np.random.seed(parameters.planner_seed)
random.seed(parameters.planner_seed)
if parameters.v:
utils.viewer()
environment.set_motion_planner(BaseMotionPlanner(environment, 'rrt'))
# from manipulation.bodies.bodies import set_color
# set_color(environment.env.GetKinBody(goal_object_names[0]), [1, 0, 0])
start_time = time.time()
n_mp = n_ik = 0
goal_object_names, high_level_plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time,
parameters) # finds the plan
total_time_taken = time.time() - start_time
n_mp += mp
n_ik += ik
total_n_nodes = 0
total_plan = []
idx = 0
found_solution = False
timelimit = parameters.timelimit
while total_time_taken < timelimit:
goal_obj_name = goal_object_names[idx]
plan, n_nodes, status, (mp, ik) = find_plan_for_obj(goal_obj_name, high_level_plan[idx], environment,
start_time,
timelimit, parameters)
total_n_nodes += n_nodes
total_time_taken = time.time() - start_time
print goal_obj_name, goal_object_names, total_n_nodes
print "Time taken: %.2f" % total_time_taken
if total_time_taken > timelimit:
break
if status == 'HasSolution':
execute_plan(plan)
environment.initial_robot_base_pose = utils.get_body_xytheta(environment.robot)
total_plan += plan
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
idx += 1
else:
# Note that HPN does not have any recourse if this happens. We re-plan at the higher level.
goal_object_names, plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time, parameters) # finds the plan
n_mp += mp
n_ik += ik
total_plan = []
idx = 0
if idx == len(goal_object_names):
found_solution = True
break
else:
idx %= len(goal_object_names)
total_time_taken = time.time() - start_time
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
print 'plan saved'
from problem_environments.conveyor_belt_problem import create_conveyor_belt_problem
from operator_utils.grasp_utils import solveTwoArmIKs, compute_two_arm_grasp
from problem_environments.conveyor_belt_env import ConveyorBelt
from mover_library.utils import get_pick_domain, get_pick_base_pose_and_grasp_from_pick_parameters, set_robot_config, \
two_arm_pick_object, visualize_path, set_obj_xytheta, get_body_xytheta
from mover_library.samplers import randomly_place_in_region
import numpy as np
problem = ConveyorBelt(problem_idx=1)
env = problem.env
robot = env.GetRobots()[0]
env.SetViewer('qtcoin')
tobj = env.GetKinBody('tobj3')
tobj_xytheta = get_body_xytheta(tobj.GetLinks()[1])
tobj_xytheta[0, -1] = (160 / 180.0) * np.pi
set_obj_xytheta(tobj_xytheta, tobj.GetLinks()[1])
for tobj in env.GetBodies():
if tobj.GetName().find('tobj') == -1: continue
randomly_place_in_region(env, tobj,
problem.problem_config['conveyor_belt_region'])
pick_domain = get_pick_domain()
tobj = env.GetKinBody('tobj1')
def sample_grasp(target_obj):
g_config = None
i = 0
while g_config is None: