def visualize_plan(plan, oracle, display=True, save=None):
if not display and save is None: return
if save is not None and not os.path.exists(save): os.makedirs(save)
def execute():
oracle.unlock()
oracle.problem.set_viewer(oracle.env)
oracle.clear_regions()
oracle.draw_goals()
set_state(plan.start, oracle)
open_gripper(oracle) # TODO - do I always want this?
update_viewer()
if display: raw_input('Hit enter to finish' if len(plan.operators) == 0 else 'Hit enter to step')
else: sleep(1) # NOTE - Gives time for the viewer to update
if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, 0, 0)) # TODO - sometimes captures images before the viewer is ready
for i, action in enumerate(plan.operators):
if isinstance(action, StepExecutable):
for j, _ in enumerate(action.step(oracle)):
update_viewer()
if display: raw_input('Hit enter to finish' if i == len(plan.operators) - 1 else 'Hit enter to step')
else: sleep(1) # Gives time for the viewer to update
if save is not None: save_image(oracle.env, IMAGE_FORMAT%(save, i+1, j)) # TODO - sometimes captures images before the viewer is ready
if is_viewer_active(oracle.env): execute()
else: execute_viewer(oracle.env, execute)
def solve_deterministic(env, hand='left', no_window=True):
test = test0 # test0 | testBusy
exp, goal = test()
print SEPARATOR
test = PlanTest(exp, noWindow=no_window)
test.load() # TODO - do I need to pass regions here?
#test_obj = test.realWorld.objectConfs.keys()[0]
#test.realWorld.getObjectPose()
#print 'Initial collision:', test.realWorld.checkRobotCollision(test.realWorld.robotConf, test.realWorld.objectShapes[test_obj])
print 'Initial collisions:', world_check_collisions(test.realWorld)
manip_problem = real_manipulation_problem(env, test.realWorld, goal, hand)
#manip_problem.set_viewer(env)
oracle = ManipulationOracle(manip_problem,
env,
active_arms=[hand],
reset_robot=False)
stream_problem = compile_problem(oracle)
oracle.draw_goals(
) # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(oracle.env):
raw_input('Start?')
search_fn = get_fast_downward(
'eager', verbose=False
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = focused_planner(stream_problem,
search=search_fn,
greedy=False,
stream_cost=10,
verbose=False,
debug=False)
#plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False)
if plan is None:
return
print plan
if no_window:
visualize_plan(Plan(
convert_state(oracle, stream_problem.initial_atoms),
executable_plan(oracle, plan)),
oracle,
display=True,
save=None)
def visualize_states(states, oracle, display=True, save=None):
if not display and save is None: return
if save is not None and not os.path.exists(save): os.makedirs(save)
def execute():
oracle.unlock()
oracle.problem.set_viewer(oracle.env)
oracle.clear_regions()
oracle.draw_goals()
for i, state in enumerate(states):
set_state(state, oracle)
oracle.env.UpdatePublishedBodies() # Updates display if env locked
if display: raw_input('Hit enter to finish' if i == len(states) - 1 else 'Hit enter to step')
else: sleep(1) # Gives time for the viewer to update
if save is not None: save_image(oracle.env, (save + 'image%0' + str(len(str(len(states) - 1))) + 'd.jpg') % i) # Sometimes captures images before the viewer is ready
if is_viewer_active(oracle.env): execute()
else: execute_viewer(oracle.env, execute)
def execute_plan(plan, oracle, pause=True):
if PRE_SMOOTH_TRAJECTORIES:
smooth_actions(plan, oracle)
def execute():
oracle.unlock()
oracle.problem.set_viewer(oracle.env)
oracle.clear_regions()
oracle.draw_goals()
#oracle.env.StartSimulation(time_step, realtime=False) # realtime=False calls simulate step as fast as possible
set_state(plan.start, oracle)
raw_input('Hit enter to finish' if len(plan.operators) == 0 else 'Hit enter to start')
for i, action in enumerate(plan.operators):
if isinstance(action, Executable):
action.execute(oracle)
if i == len(plan.operators) - 1: raw_input('Hit enter to finish')
elif pause: raw_input('Hit enter to continue')
if is_viewer_active(oracle.env): execute()
else: execute_viewer(oracle.env, execute)
def solve_tamp(env, mproblem, display, incremental, focused, movie):
RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug)
mproblem.set_viewer(env)
mproblem.load_env(env)
oracle = ManipulationOracle(mproblem, env)
stream_problem = compile_problem(oracle)
print stream_problem
oracle.draw_goals() # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(oracle.env):
raw_input('Start?')
search_fn = get_fast_downward('eager', verbose=False) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
verbose = False
debug = False
profile = False
assert incremental != focused
planner = INCREMENTAL if incremental else FOCUSED
#planner = 'hierarchy' # 'hierarchy' | 'serial'
t0 = time()
def solve():
if planner == INCREMENTAL:
return incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, verbose=verbose, debug=debug) # 1 | 20 | 100 | INF
elif planner == FOCUSED:
return focused_planner(stream_problem, search=search_fn, greedy=False, stream_cost=10, verbose=verbose, debug=debug)
elif planner == 'serial':
return goal_serialization(stream_problem, verbose=verbose, debug=debug)
elif planner == 'hierarchy':
selector = first_selector # first_selector | all_selector
return replan_hierarchy(stream_problem, selector=selector, first_only=False, execute=True, verbose=verbose)
else:
raise ValueError(planner)
if profile:
from misc.profiling import run_profile
(plan, universe), prof = run_profile(solve)
else:
(plan, universe), prof = solve(), None
if prof is not None:
from misc.profiling import str_profile
print SEPARATOR
print str_profile(prof)
universe.print_statistics()
#print_plan_stats(plan, universe)
print SEPARATOR
print 'Planner:', planner
print 'Search:', FAST_DOWNWARD
print 'Solved:', plan is not None
print 'Length:', len(plan) if plan is not None else None
print 'Time:', time() - t0
if plan is not None:
print 'Plan:' #, convert_plan(plan)
for i, (action, args) in enumerate(convert_plan(plan)):
print '%s) %s%s'%(i, action, tuple(args))
if plan is None:
return
#if is_viewer_active(oracle.env):
# raw_input('Done!')
#return
#trace_plan(stream_problem, plan)
#print get_raw_states(stream_problem, plan)
images_directory = os.path.join(get_directory_path(mproblem.name, os.path.basename(__file__)), 'images') if movie else None
if display:
#display_solution(oracle, plan, directory_path, 'plan', movie)
#states = map(lambda s: convert_state(oracle, s), get_states(pddl_problem, plan))
#visualize_states(states, oracle, display=True, save=images_directory)
start = convert_state(oracle, stream_problem.initial_atoms)
executable = executable_plan(oracle, plan)
visualize_plan(Plan(start, executable), oracle, display=True, save=images_directory)
def solve_constraint_tamp(env, manip_problem, display, focused, movie, execute = True, verbose=False):
#RaveSetDebugLevel(DebugLevel.Fatal) #RaveSetDebugLevel(DebugLevel.Debug)
manip_problem.set_viewer(env)
manip_problem.load_env(env)
oracle = ManipulationOracle(manip_problem, env)
fts_problem = tamp_problem(oracle)
print
print fts_problem
stream_problem = constraint_to_stripstream(fts_problem)
print
print stream_problem
oracle.draw_goals() # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(oracle.env):
raw_input('Start?')
search = get_fast_downward('eager') # 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
planner = FOCUSED if focused else INCREMENTAL
print SEPARATOR
t0 = time()
if planner == FOCUSED:
# TODO - why do I need make_stream_instances=True here?
plan, _ = focused_planner(stream_problem, search=search, check_feasible=False, greedy=False, dfs=True, verbose=verbose)
elif planner == INCREMENTAL:
frequency = 1 # 1 | 20 | 100 | INF
# NOTE - the versions I submitted to RSS had implicitly waves=False
plan, _ = incremental_planner(stream_problem, search=search, frequency=frequency, waves=True, verbose=verbose)
else:
raise ValueError('Must specify a planner')
print SEPARATOR
print 'Planner:', planner
print 'Search:', FAST_DOWNWARD
print 'Solved:', plan is not None
print 'Length:', len(plan) if plan is not None else None
print 'Time:', time() - t0
if plan is None:
return
# NOTE - need to set constants
#PRE_SMOOTH_TRAJECTORIES = True
#REPLAN_TRAJECTORIES = True
#SMOOTH_TRAJECTORIES = identity_trajectories # identity_trajectories | smooth_trajectories
#print oracle.robot.GetAffineTranslationMaxVels(), oracle.robot.GetAffineRotationAxisMaxVels()
#oracle.robot.SetAffineTranslationMaxVels(5*oracle.robot.GetAffineTranslationMaxVels())
#SetAffineRotationAxisMaxVels(oracle.robot) # SetAffineRotationAxisMaxVels | SetAffineRotation3DMaxVels
#SetAffineTranslationMaxVels(oracle.robot)
# SetActiveDOFVelocities, SetDOFVelocities
# SetDOFAccelerationLimits
# GetActiveDOFMaxAccel but not SetActiveDOFMaxAccel
#oracle.robot.SetActiveDOFVelocities(5*oracle.robot.GetActiveDOFVelocities())
#action.base_trajs[0].cspace.set_active()
#indices = oracle.robot.GetActiveDOFIndices()
#print indices
#print oracle.robot.GetActiveDOFMaxAccel(indices)
#oracle.robot.SetActiveDOFMaxAccel(5*oracle.robot.GetActiveDOFMaxAccel(indices), indices)
# NOTE - not sure why the focused algorithm isn't working that well here anymore
images_directory = None
if movie:
directory_path = get_directory_path(manip_problem.name, 'fts')
images_directory = os.path.join(directory_path, 'images/')
print 'Save:', images_directory
if display or movie:
Plan = namedtuple('Plan', ['start', 'operators'])
start = convert_state(oracle, get_single_state(fts_problem.initial_state))
executables = convert_plan(oracle, plan)
if execute:
if movie:
from misc.utils import launch_quicktime
launch_quicktime() # TODO - produces missing value if quicktime is already enabled
execute_plan(Plan(start, executables), oracle, pause=False)
else:
visualize_plan(Plan(start, executables), oracle, display=display, save=images_directory)
def policy(self, belief, goal):
# The positions of objects don't change as the robot moves (i.e they are in a global frame)
# The variances grow though as the robot moves
# The robot config has no uncertainty
# Thus, things are sort of relative to the robot
# Sample poses that are reachable given the uncertainty and can see the table
# Can ignore transition uncertainty for action effects
# Only need to keep track of uncertainty along hte first move action as a precondition
# Can either sample robot trajectory rollouts or object poses as the robot moves with increased uncertainty
# Just do large objects for base collisions
from manipulation.primitives.transforms import vector_trans
from manipulation.bodies.robot import approach_vector_from_object_trans
DISPLAY_GRASPS_HPN = False
if DISPLAY_GRASPS_HPN:
obj = 'objA'
conf = belief.pbs.getConf(
) # belief.conf # Default conf (i.e. base is wrong)
grasp_desc = belief.objects[obj].attrs['graspDesc']
shape = belief.objects[obj].attrs['shape']
print shape.parts()[0].vertices()
import util.windowManager3D as wm
win = 'W'
for i in range(len(grasp_desc)):
wm.getWindow(win).clear()
obj_trans = get_object_frame(conf, self.hand, grasp_desc[i])
#approach_trans = get_approach_frame(conf, self.hand)
#print np.round(approach_trans.matrix, 2)
approach_vector = or_from_hpn_approach_vector(
conf.robot, self.hand, grasp_desc[i])
transformed_vector = approach_vector_from_object_trans(
obj_trans.matrix, approach_vector)
obj_trans = hu.Transform(
vector_trans(obj_trans.matrix, transformed_vector))
#temp_matrix = obj_trans.matrix.copy() # NOTE - this works as well?
#temp_matrix[:3, 3] = approach_trans.matrix[:3, 3]
#obj_trans = hu.Transform(temp_matrix)
#print shape.origin() # Origin
#print np.round(obj_trans.matrix, 2)
shape.applyTrans(obj_trans).draw(win, color='blue')
conf.draw(win, color='red')
raw_input('Continue?')
return None
assert not check_belief_collisions(belief, .9)
if satisfies(belief, goal):
return []
if self.plan is None:
env = get_env()
manip_problem = or_manipulation_problem(env, belief, goal)
#manip_problem.set_viewer(env)
self.oracle = ManipulationOracle(
manip_problem, env, active_arms=[self.hand], reset_robot=False
) # TODO - need to avoid resetting the original configuration
# TODO - try just replacing the current problem
DISPLAY_GRASPS_OR = False
if DISPLAY_GRASPS_OR: # NOTE - OR Grasps are from Manip to Pose (not Gripper)
from manipulation.bodies.robot import get_manip_trans, approach_vector_from_object_trans
from manipulation.primitives.transforms import object_trans_from_manip_trans, set_trans, point_from_trans
from manipulation.grasps.grasps import get_grasps
from manipulation.primitives.display import draw_arrow
obj = 'objA'
#set_trans(self.oracle.bodies[obj], np.eye(4))
#print self.oracle.bodies[obj].ComputeAABB()
#for grasp in get_grasps(self.oracle, obj): # NOTE - this randomizes the grasps
for grasp in get_or_grasps(self.oracle, obj):
manip_trans = get_manip_trans(self.oracle)
#grasp_trans = or_from_hpn_grasp(belief.conf.robot, self.hand, grasp_desc[i]).matrix
obj_trans = object_trans_from_manip_trans(
manip_trans, grasp.grasp_trans)
set_trans(self.oracle.bodies[obj], obj_trans)
approach_vector = approach_vector_from_object_trans(
obj_trans, grasp.approach_vector)
approach_trans = vector_trans(obj_trans, approach_vector)
_ = draw_arrow(env, point_from_trans(approach_trans),
point_from_trans(obj_trans))
print
print grasp.approach_vector
print grasp.grasp_index
print np.round(obj_trans, 2)
raw_input('Continue?')
return None
# TODO - check that the plan is still good by converting the state
stream_problem = compile_problem(self.oracle)
self.oracle.draw_goals(
) # NOTE - this must be after compile_problem to ensure the goal_poses convert
if is_viewer_active(self.oracle.env):
raw_input('Start?')
search_fn = get_fast_downward(
'eager', verbose=False
) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = focused_planner(stream_problem,
search=search_fn,
greedy=False,
stream_cost=10,
verbose=False,
debug=False)
#plan, universe = incremental_planner(stream_problem, search=search_fn, verbose=True, debug=False)
if plan is None:
plan = []
#visualize_plan(Plan(convert_state(self.oracle, stream_problem.initial_atoms),
# executable_plan(self.oracle, plan)), self.oracle, display=True, save=None)
#for state in get_states(universe, plan):
# or_state = convert_state(self.oracle, state)
# set_state(or_state, self.oracle)
# raw_input('Continue?')
look_iterations = 3
self.plan = []
state_sequence = get_states(universe, plan)
for i, (action, args) in enumerate(convert_plan(plan)):
or_state = convert_state(self.oracle, state_sequence[i])
set_state(or_state, self.oracle)
if action.name in ['move']:
last_config = self.oracle.get_robot_config()
for _ in range(look_iterations):
for obj in belief.objects:
if or_state.get(obj, True) is not None:
result = look_at_ik(self.oracle, obj)
if result is not None:
look_config = self.oracle.get_robot_config(
)
self.plan += [
('move_no_base', (last_config,
look_config)),
('look', (obj, look_config)),
]
last_config = look_config # TODO - does this mean I have to change the next action config?
#print 'Looking at', obj
#raw_input('Pause')
self.plan.append((action.name, args))
elif action.name in ['pick', 'place']:
obj, pose, grasp, approach_config, pap = args
self.plan += [
('move_no_base', (approach_config, pap.vector_config)),
('move_no_base', (pap.vector_config,
pap.grasp_config)),
(action.name, args),
('move_no_base', (pap.grasp_config,
pap.vector_config)),
('move_no_base', (pap.vector_config, approach_config)),
]
if self.replan:
break
else:
self.plan.append((action.name, args))
#raw_input('Continue?')
print self.plan
if not self.plan:
return None
print SEPARATOR
action, args = self.plan.pop(0)
print action, args
if not self.plan and self.replan:
self.plan = None
return [self.convert_action(belief, action, args)
] # NOTE - need to return a list for now