def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan):
def execute_traj(traj):
#for j, conf in enumerate(traj.path()):
#for j, conf in enumerate([traj.end()]):
path = list(sample_manipulator_trajectory(manipulator, traj.traj()))
for j, conf in enumerate(path):
set_manipulator_conf(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(path)))
# Resets the initial state
set_manipulator_conf(manipulator, initial_conf.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
_, _, traj = args
execute_traj(traj)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
execute_traj(traj)
elif action.name == 'pick':
obj, _, _, _, traj = args
execute_traj(traj.reverse())
robot.Grab(bodies[obj])
execute_traj(traj)
elif action.name == 'place':
obj, _, _, _, traj = args
execute_traj(traj.reverse())
robot.Release(bodies[obj])
execute_traj(traj)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(body2):
return False
return True
def _cfree_traj_pose(traj, pose):
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
#print manipulator.CheckEndEffectorCollision(manip_trans)
if grasp_config is not None:
yield [Config(grasp_config)]
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2 = map(get_value, self.inputs)
collision_atoms = filter(
lambda atom: atom.predicate in
[CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is CFreeMTrajGrasp:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is CFreeMTrajPose:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
#print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies)
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies) - 1
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
grasp.grasp_trans))
robot.Grab(body1)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [
atom.instantiate({collision_params[atom]: MTRAJ(mt)})
for atom in collision_atoms
]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if mt:
return [ManipMotion(mq1, mq2, mt)]
return []
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
set_default_robot_config(env.GetRobots()[0])
set_point(env.GetRobots()[0], (-1.5, 0, 0))
m, n = 3, 3
#m, n = 5, 5
n_obj = 8
side_dim = .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
pose_indices = list(product(range(m), range(n)))
colors = {}
for r, c in pose_indices:
color = np.zeros(4)
color[2-r] = 1.
colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0])
poses = {}
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r, c in pose_indices:
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))
initial_indices = randomize(pose_indices[:])
initial_poses = {}
goal_poses = {}
for i, indices in enumerate(pose_indices[:n_obj]):
name = 'block%d-%d'%indices
color = colors[indices]
initial_poses[name] = poses[initial_indices[i]]
goal_poses[name] = poses[indices]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, initial_poses[name].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=poses.values())
def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose
enable_all(all_bodies, False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.path():
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding
if DISABLE_TRAJECTORIES:
yield [(object(),)] # [(True,)]
return
enable_all(all_bodies, False)
body1.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans))
robot.Grab(body1)
#traj = motion_plan(env, cspace, conf2.value, self_collisions=True)
traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
robot.Release(body1)
if not traj: return
traj.grasp = grasp
yield [(traj,)]
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration
if grasp_conf is None: return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration
yield [(pregrasp_conf, grasp_traj)]
def sample_grasp_traj(pose, grasp):
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans)
if grasp_conf is None:
return
if DISABLE_TRAJECTORIES:
yield [(Conf(grasp_conf), object())]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
grasp_traj = vector_traj_helper(env, robot, approach_vector)
robot.Release(body1)
if grasp_traj is None:
return
grasp_traj.grasp = grasp
pregrasp_conf = Conf(grasp_traj.end())
yield [(pregrasp_conf, grasp_traj)]
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(robot, body2):
print 'cfree_mtraj_pose'
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
print 'cfree_mtraj_grasp_pose'
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_manip_motion(mq1, mq2):
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if not mt: return
yield [(mt,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[],
effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose, eager=True),
EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(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)
#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'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2 body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2)
def rearrangement_problem(env,obj_poses=None):
# import random
# seed = 50
# random.seed(seed)
# np.random.seed(seed)
MIN_DELTA = .01 # .01 | .02
ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml'
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]
TABLE_NAME = 'table1'
NUM_OBJECTS = 8
WIDTH = .05 # .07 | .1
TARGET_WIDTH = 0.07
OBJ_HEIGHT = 0.2
objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \
color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)]
target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \
color=(1, 1.5, 0))
#TODO: Place obstacle that prevent you to reach from top
OBST_X = 0
OBST_WIDTH = 1.
OBST_COLOR = (1, 0, 0)
OBST_HEIGHT = 0.4
OBST_TRANSPARENCY = .25
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME)
place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(.225, OBST_X, 0), TABLE_NAME)
# roof
OBST_Z = OBST_HEIGHT
place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY),
(0, OBST_X, OBST_Z,0), TABLE_NAME)
# I think this can be done once and for all
REST_TORSO = [.15]
robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')])
l_model = databases.linkstatistics.LinkStatisticsModel(robot)
if not l_model.load(): l_model.autogenerate()
l_model.setRobotWeights()
min_delta = 0.01
l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object
extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T
robot.SetAffineTranslationLimits(*extrema)
X_PERCENT = 0.0
# define intial region for movable objects
init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5)))
#init_region.draw(env)
# define target object region
target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \
TABLE_NAME, color=np.array((0, 0, 0, 0.9)))
target_obj_region.draw(env)
# place object
if obj_poses is not None:
for obj in objects:
set_pose(obj,obj_poses[get_name(obj)])
set_quat(obj, quat_from_z_rot(0))
if env.GetKinBody(get_name(obj)) is None: env.Add(obj)
set_pose( target_obj, obj_poses[get_name(target_obj)] )
set_quat( target_obj, quat_from_z_rot(0) )
if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj)
else:
for obj in objects:
randomly_place_region(env, obj, init_region)
set_quat(obj, quat_from_z_rot(0))
randomly_place_region(env, target_obj, target_obj_region)
set_quat(target_obj, quat_from_z_rot(0))
object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left)
set_xy(robot, -.75, 0)
# LEFT_ARM_CONFIG = HOLDING_LEFT_ARM
robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices())
robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices())
grasps = {}
for obj_name in object_names:
obj = env.GetKinBody(obj_name)
obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \
+ get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)
grasps[get_name(obj)] = obj_grasps
target_obj = env.GetKinBody('target_obj')
target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\
get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH)
grasps['target_obj'] = target_obj_grasps
initial_poses={}
for obj in objects:
initial_poses[get_name(obj)] = get_pose(obj)
initial_poses[get_name(target_obj)] = get_pose(target_obj)
return ManipulationProblem(None,
object_names=object_names, table_names='table1',
goal_regions=init_region,grasps=grasps,
initial_poses = initial_poses)
def solve_tamp(env):
viewer = env.GetViewer() is not None
#problem = dantam(env)
problem = dantam2(env)
#problem = move_several_4(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, 'leftarm')
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE
USE_GRASP_APPROACH = GRASP_APPROACHES.TOP
#USE_GRASP_TYPE = GRASP_TYPES.TOUCH
USE_GRASP_TYPE = GRASP_TYPES.GRASP
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
##################################################
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
def collision_free(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
##################################################
"""
class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2
def get_values(self, **kwargs):
self.enumerated = True
pose1, pose2 = map(get_value, self.inputs)
if collision_free(pose1, pose2):
return [PoseCFree(pose1, pose2)]
return []
#return [Movable()] # NOTE - only make movable when it fails a collision check
CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways
class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now
movable = []
def get_values(self, **kwargs):
self.enumerated = True
pose, = map(get_value, self.inputs)
results = [CheckedInitial(pose)]
for obj, pose2 in problem.initial_poses.iteritems():
if obj not in self.movable and collision_free(pose, pose2):
pass
#results.append(PoseCFree(pose, pose2))
else:
self.movable.append(obj)
results.append(PoseEq(obj, pose2))
#results.append(Movable(obj))
if results:
pass # NOTE - I could make this fail if there is a collision
# I could prevent the binding by directly adding CheckedInitial to the universe
# In general, I probably can just mutate the problem however I see fit here
return results
"""
##################################################
# NOTE - can do pose, approach manip, true approach traj, motion plan
# NOTE - can make something that produces approach trajectories
def get_manip_vector(pose, grasp):
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
#enable_all(False)
#if manipulator.CheckEndEffectorCollision(manip_trans):
# return None
return ManipVector(manip_trans, approach_vector)
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
#print manipulator.CheckEndEffectorCollision(manip_trans)
if grasp_config is not None:
yield [Config(grasp_config)]
#traj = workspace_traj_helper(base_manip, approach_vector)
def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
##################################################
# NOTE - can either include the held object in the traj or have a special condition that not colliding
def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in
yield None
#enable_all(False)
#with robot:
# set_base_values(robot, bq.value)
# pass
# TODO - does it make sense to make a new stream for the biasing or to continuously just pass things
# I suppose I could cache the state or full plan as context
class MotionStream(Stream): # TODO - maybe make this produce the correct values
num = 0
#def get_values(self, **kwargs):
# self.enumerated = True
# mq1, mq2, bq = map(get_value, self.inputs)
# #mt = None
# mt = MotionStream.num
# MotionStream.num += 1 # Ensures all are unique
# return [ManipMotion(mq1, mq2, bq, mt)]
def sample_motion_plan(self, mq1, mq2, bq):
set_manipulator_values(manipulator, mq1.value)
set_base_values(robot, bq.value)
return motion_plan(env, cspace, mq2.value, self_collisions=True)
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2, bq = map(get_value, self.inputs)
collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is MTrajGraspCFree:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is MTrajPoseCFree:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj?
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies)-1
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans))
robot.Grab(body1)
mt = self.sample_motion_plan(mq1, mq2, bq)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
mt = self.sample_motion_plan(mq1, mq2, bq)
if mt:
return [ManipMotion(mq1, mq2, bq, mt)]
return []
##################################################
cond_streams = [
#MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses),
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj),
ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0),
#MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[],
# effects=[Kin(P, G, BQ, MQ)], generator=sample_ik),
EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[],
effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj),
#EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)],
# test=collision_free, eager=True),
EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)],
test=collision_free, eager=True),
#ClassStream(inputs=[P, P2], conditions=[], outputs=[],
# effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True),
EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)],
test=lambda mt, p: True, eager=True),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)],
test=lambda mt, g: True, eager=True),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)],
test=lambda mt, g, p: True, eager=True),
#ClassStream(inputs=[P], conditions=[], outputs=[],
# effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream),
]
##################################################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_full = Config(get_full_config(robot))
initial_base = get_base_conf(initial_full)
initial_manip = get_arm_conf(manipulator, initial_full)
initial_atoms = [
BaseEq(initial_base),
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
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'
##################################################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
open_gripper2(manipulator)
set_base_values(robot, initial_base.value)
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move_arm':
mq1, mq2, bq, mt = args
#set_manipulator_values(manipulator, mq2.value)
step_path(mt)
elif action.name == 'pick':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
#grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
#open_gripper2(manipulator)
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach)
assert REARRANGEMENT
env.Load(ENVIRONMENTS_DIR + 'empty.xml')
m, n = 3, 3
#m, n = 5, 5
side_dim = .07 # .05 | .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
#separation = (side_dim/2, side_dim/2)
coordinates = list(product(range(m), range(n)))
assert n_obj <= len(coordinates)
obj_coordinates = sample(coordinates, n_obj)
length = m*(box_dims[0] + separation[0])
width = n*(box_dims[1] + separation[1])
height = .7
table = box_body(env, length, width, height, name='table', color=get_color('tan1'))
set_point(table, (0, 0, 0))
env.Add(table)
robot = env.GetRobots()[0]
set_default_robot_config(robot)
set_base_values(robot, (-1.5, 0, 0))
#set_base_values(robot, (0, width/2 + .5, math.pi))
#set_base_values(robot, (.35, width/2 + .35, math.pi))
#set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4))
poses = []
z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET
for r in range(m):
row = []
x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0])
for c in range(n):
y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1])
row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))))
poses.append(row)
initial_poses = {}
goal_poses = {}
# TODO - randomly assign goal poses here
for i, (r, c) in enumerate(obj_coordinates):
row_color = np.zeros(4)
row_color[2-r] = 1.
if i == 0:
name = 'goal%d-%d'%(r, c)
color = BLUE
goal_poses[name] = poses[m/2][n/2]
else:
name = 'block%d-%d'%(r, c)
color = RED
initial_poses[name] = poses[r][c]
obj = box_body(env, *box_dims, name=name, color=color)
set_pose(obj, poses[r][c].value)
env.Add(obj)
#for obj in randomize(objects):
# randomly_place_body(env, obj, [get_name(table)])
known_poses = list(flatten(poses))
#known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here
return ManipulationProblem(function_name(inspect.stack()),
object_names=initial_poses.keys(), table_names=[get_name(table)],
goal_poses=goal_poses,
initial_poses=initial_poses, known_poses=known_poses)
def cfree_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def get_aabb(self, body_name):
body = self.env.GetKinBody(body_name)
with body: # NOTE - before I didn't have this and it worked for rlplan
set_pose(body, unit_pose())
return aabb_from_body(self.env.GetKinBody(body_name))
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi / 2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {
body.GetKinematicsGeometryHash()
for body in bodies.values()
}
assert len(
geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH,
USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(
gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(body2):
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
class MotionStream(Stream):
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2 = map(get_value, self.inputs)
collision_atoms = filter(
lambda atom: atom.predicate in
[CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is CFreeMTrajGrasp:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is CFreeMTrajPose:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
#print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies)
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies) - 1
set_pose(
body1,
object_trans_from_manip_trans(get_trans(manipulator),
grasp.grasp_trans))
robot.Grab(body1)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [
atom.instantiate({collision_params[atom]: MTRAJ(mt)})
for atom in collision_atoms
]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if mt:
return [ManipMotion(mq1, mq2, mt)]
return []
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[MQ, GT],
conditions=[],
effects=[GraspMotion(P, G, MQ, GT)],
generator=sample_grasp_traj),
# Move trajectory
ClassStream(inputs=[MQ, MQ2],
outputs=[MT],
conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)],
StreamClass=MotionStream,
order=1,
max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose,
eager=True),
EasyTestStream(inputs=[GT, P],
conditions=[],
effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P],
conditions=[],
effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G],
conditions=[],
effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P],
conditions=[],
effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ManipEq(initial_manip),
*(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')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
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'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?' % (j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?' % (i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
