def load_env(env):
viewer = env.GetViewer()
if viewer is not None:
viewer.SetName('Simulation')
viewer.SetCamera(camera_look_at((0, -2.5, 5), look_point=(0, 0, 0)),
focalDistance=5.0)
viewer.SetSize(640, 480)
viewer.Move(0, 0)
#robot = env.ReadRobotXMLFile('robots/pr2-beta-static.zae')
#robot = env.ReadRobotXMLFile('robots/pr2-beta-sim.robot.xml')
or_robot = env.ReadRobotXMLFile('robots/pr2-beta-static.zae')
robot = env.ReadRobotXMLFile('robotics/openrave/environments/robot.dae')
env.Add(robot)
for manipulator in or_robot.GetManipulators():
robot.AddManipulator(manipulator.GetInfo())
for link_name in IGNORE_LINKS:
link = robot.GetLink(link_name)
link.SetVisible(False)
link.Enable(False)
set_manipulator_conf(robot.GetManipulator('leftarm'), DEFAULT_LEFT_ARM)
open_gripper(robot.GetManipulator('leftarm'))
set_manipulator_conf(robot.GetManipulator('rightarm'), REST_RIGHT_ARM)
# mirror_arm_config(robot, REST_LEFT_ARM))
#close_gripper(robot.GetManipulator('rightarm'))
open_gripper(robot.GetManipulator('rightarm'))
robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
robot.SetAffineTranslationLimits(*(MAX_DISTANCE *
np.array([[-1, -1, 0], [1, 1, 0]])))
vel_multi = 0.25
robot.SetDOFVelocityLimits(vel_multi * robot.GetDOFVelocityLimits())
robot.SetAffineTranslationMaxVels(vel_multi *
robot.GetAffineTranslationMaxVels())
robot.SetAffineRotationAxisMaxVels(vel_multi *
robot.GetAffineRotationAxisMaxVels())
collision_checker = ODE if RaveCreateCollisionChecker(env,
FCL) is None else FCL
_, arm, base_manip, _ = initialize_openrave(
env, 'leftarm', min_delta=.01, collision_checker=collision_checker)
print 'Using collision checker', env.GetCollisionChecker()
voxels = []
if OCTREE_RESOLUTION is not None:
# TODO: add legs of tables
extent = 3 * (OCTREE_RESOLUTION, )
centers = []
for z in np.arange(OCTREE_MIN_HEIGHT, OCTREE_MAX_HEIGHT,
OCTREE_RESOLUTION):
for t in np.arange(-MAX_DISTANCE, MAX_DISTANCE, OCTREE_RESOLUTION):
if np.random.random() < 0.9:
centers += [(t, MAX_DISTANCE, z), (t, -MAX_DISTANCE, z),
(MAX_DISTANCE, t, z), (-MAX_DISTANCE, t, z)]
voxels.append((extent, centers))
return robot, Occupancy(voxels, unit_pose())
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, _ = initialize_openrave(env,
ARM,
min_delta=.01)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
fts_problem = get_problem(env, problem, robot, manipulator, base_manip,
bodies, initial_conf)
print
print fts_problem
stream_problem = constraint_to_stripstream(fts_problem)
print
print stream_problem
for stream in stream_problem.cond_streams:
print stream, stream.max_level
# TODO - why is this slower/less reliable than the other one (extra axioms, eager evaluation?)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
def solve_tamp(env, use_focused):
use_viewer = env.GetViewer() is not None
problem = PROBLEM(env)
# TODO: most of my examples have literally had straight-line motions plans
robot, manipulator, base_manip, ir_model = initialize_openrave(
env, ARM, min_delta=MIN_DELTA)
set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
surfaces = {surface.name: surface for surface in problem.surfaces}
open_gripper(manipulator)
initial_q = Conf(robot.GetConfigurationValues())
initial_poses = {
name: Pose(name, get_pose(body))
for name, body in bodies.iteritems()
}
# TODO: just keep track of the movable degrees of freedom
# GetActiveDOFIndices, GetActiveJointIndices, GetActiveDOFValues
saver = EnvironmentStateSaver(env)
#cfree_pose = cfree_pose_fn(env, bodies)
#cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies)
#base_generator = base_generator_fn(ir_model)
#base_values = base_values_from_full_config(initial_q.value)
#goal_values = full_config_from_base_values(base_values + np.array([1, 0, 0]), initial_q.value)
#goal_conf = Conf(goal_values)
#return
####################
# TODO - should objects contain their geometry
cond_streams = [
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[IsPose(O, P),
IsGrasp(O, G)],
effects=[GraspMotion(O, P, G, Q, T)],
generator=sample_grasp_traj_fn(
base_manip, ir_model, bodies, CARRY_CONFIG)),
EasyGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_base_motion_fn(base_manip, bodies),
order=1,
max_level=0),
EasyTestStream(
inputs=[O, P, O2, P2],
conditions=[IsPose(O, P), IsPose(O2, P2)],
effects=[CFreePose(P, P2)],
test=lambda *args: True, #cfree_pose,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=lambda *args: True),
#test=cfree_traj),
EasyListGenStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
generator=grasp_generator_fn(bodies, TOP_GRASPS,
SIDE_GRASPS,
MAX_GRASPS)),
EasyListGenStream(inputs=[O, S],
outputs=[P],
conditions=[],
effects=[IsPose(O, P), Stable(P, S)],
generator=pose_generator_fn(bodies, surfaces)),
#EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
# effects=[], generator=base_generator),
]
####################
constants = []
initial_atoms = [ConfEq(initial_q), HandEmpty()]
for name in initial_poses:
initial_atoms += body_initial_atoms(name, initial_poses, bodies,
surfaces)
goal_formula = And(
ConfEq(initial_q), *[
OnSurface(obj, surface)
for obj, surface in problem.goal_surfaces.iteritems()
])
#goal_formula = ConfEq(goal_conf)
#obj, _ = problem.goal_surfaces.items()[0]
#goal_formula = And(Holding(obj))
#goal_formula = Holding(obj) # TODO: this cause a bug
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
print stream_problem
handles = draw_affine_limits(robot)
if use_viewer:
for surface in problem.surfaces:
surface.draw(env)
raw_input('Start?')
max_time = INF
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
if use_focused:
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False,
max_time=max_time)
else:
solve = lambda: incremental_planner(stream_problem,
search=search_fn,
frequency=10,
waves=False,
debug=False,
max_time=max_time)
with env:
plan, universe = solve()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if plan is not None:
commands = process_plan(robot, bodies, plan)
if use_viewer:
print SEPARATOR
saver.Restore()
visualize_solution(commands, step=False)
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, _ = initialize_openrave(env,
ARM,
min_delta=.01)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash()
for body in all_bodies
}) == 1 # Assuming all objects has the same geometry
body1 = all_bodies[-1] # Generic object 1
body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else []
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[Q, T],
conditions=[],
effects=[GraspMotion(P, G, Q, T)],
generator=sample_grasp_traj_fn(
env, manipulator, body1, all_bodies)),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_motion_fn(
manipulator, base_manip, all_bodies),
order=1,
max_level=0),
EasyListGenStream(inputs=[Q, Q2, G],
outputs=[T],
conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)],
generator=sample_holding_motion_fn(
manipulator, base_manip, body1, all_bodies),
order=1,
max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePose(P, P2)],
test=cfree_pose_fn(env, body1, body2),
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=cfree_traj_fn(env, manipulator, body1, body2,
all_bodies)),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ConfEq(initial_conf),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
def solve_tamp(env):
use_viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, ir_model = initialize_openrave(
env, ARM, min_delta=MIN_DELTA)
set_manipulator_conf(ir_model.manip, CARRY_CONFIG)
bodies = {obj: env.GetKinBody(obj) for obj in problem.movable_names}
surfaces = {surface.name: surface for surface in problem.surfaces}
open_gripper(manipulator)
initial_conf = Conf(robot.GetConfigurationValues())
initial_poses = {
name: Pose(get_pose(body))
for name, body in bodies.iteritems()
}
saver = EnvironmentStateSaver(env)
stable_test = get_stable_test(bodies, surfaces)
####################
cond_streams = [
# # Pick/place trajectory
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[IsPose(O, P),
IsGrasp(O, G)],
effects=[GraspMotion(O, P, G, Q, T)],
generator=sample_grasp_traj_fn(base_manip,
ir_model,
bodies,
CARRY_CONFIG,
max_failures=200)),
#
# # Move trajectory
#MultiEasyGenStream(inputs=[Q, Q2], outputs=[T], conditions=[],
# effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0),
#MultiEasyGenStream(inputs=[Q, Q2, O, G], outputs=[T], conditions=[IsGrasp(O, G)],
# effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0),
#
# # Collisions
EasyTestStream(inputs=[O, P, O2, P2],
conditions=[IsPose(O, P), IsPose(O2, P2)],
effects=[CFreePose(P, P2)],
test=lambda *args: True,
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=lambda *args: True),
EasyListFnStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[IsGrasp(O, G)],
function=grasp_generator_fn(bodies)),
#EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[],
# effects=[IsPose(O, P), Stable(P, S)],
# generator=pose_generator_fn(bodies, surfaces)),
#EasyListGenStream(inputs=[O, S], outputs=[P], conditions=[],
# effects=[IsPose(O, P), Stable(P, S), IsEdge(P)],
# generator=edge_generator_fn(bodies, surfaces)),
# TODO: remove O from here
RawListGenStream(
inputs=[O, P, P2],
outputs=[Q, T],
#conditions=[IsPushable(O), IsPose(O, P), IsPose(O, P2), Stable(P, S), Stable(P2, S)],
conditions=[IsPose(O, P), IsPose(O, P2)],
effects=[PushMotion(O, P, P2, Q, T)],
generator=sample_push_traj_fn(ir_model, bodies, surfaces,
CARRY_CONFIG)),
# TODO: could have fixed obstacle blocking a push
#MultiEasyGenStream(inputs=[O, S, P], outputs=[P2],
# conditions=[IsPose(O, P), Stable(P, S)],
# effects=[PushMotion(O, P, P2, Q, T)],
# generator=edge_generator_fn(bodies, surfaces)),
#EasyGenStream(inputs=[O, P, G], outputs=[Q], conditions=[IsPose(O, P), IsGrasp(O, G)],
# effects=[], generator=base_generator),
]
####################
constants = []
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PointEq(name, pose) for name, pose in initial_poses.iteritems()
] + [IsPose(name, pose)
for name, pose in initial_poses.iteritems()] + [
Stable(pose, surface)
for obj, pose in initial_poses.iteritems()
for surface in surfaces if stable_test(obj, pose, surface)
]
#goal_formula = And(ConfEq(initial_conf), *[
# OnSurface(obj, surface) for obj, surface in problem.goal_surfaces.iteritems()
#])
#goal_formula = ConfEq(goal_conf)
obj, _ = problem.goal_surfaces.items()[0]
goal_formula = And(Holding(obj))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
print stream_problem
handles = draw_affine_limits(robot)
if use_viewer:
for surface in problem.surfaces:
surface.draw(env)
raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
solve = lambda: incremental_planner(
stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
with env:
plan, universe = solve()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if plan is not None:
commands = process_plan(robot, bodies, plan)
if use_viewer:
print SEPARATOR
saver.Restore()
visualize_solution(commands)
raw_input('Finish?')