def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies,
plan):
def _execute_traj(confs):
for j, conf in enumerate(confs):
set_manipulator_conf(manipulator, conf)
sleep(0.05)
#raw_input('%s/%s) Step?'%(j, len(confs)))
# 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)
raw_input('Start?')
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.value)
elif action.name == 'move_holding':
_, _, traj, _, _ = args
_execute_traj(traj.value)
elif action.name == 'pick':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Grab(bodies[obj])
_execute_traj(traj.value)
elif action.name == 'place':
obj, _, _, _, traj = args
_execute_traj(traj.value[::-1])
robot.Release(bodies[obj])
_execute_traj(traj.value)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_MOTIONS:
#traj = Traj([conf1.value, conf2.value])
traj = Traj([conf2.value])
traj.pose = None
traj.grasp = None
yield [(traj, )]
return
enable_all(all_bodies, False)
set_manipulator_conf(manipulator, conf1.value)
if has_mp():
path = mp_birrt(robot, conf1.value, conf2.value)
else:
#traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
path = manipulator_motion_plan(base_manip,
manipulator,
conf2.value,
max_iterations=10)
if path is None:
return
traj = Traj(path)
traj.pose = None
traj.grasp = None
yield [(traj, )]
def sample_holding_motion(conf1, conf2,
grasp): # Sample motion while holding
if DISABLE_MOTIONS:
#traj = Traj([conf1.value, conf2.value])
traj = Traj([conf2.value])
traj.pose = None
traj.grasp = grasp
yield [(traj, )]
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.value))
robot.Grab(body1)
if has_mp():
path = mp_birrt(robot, conf1.value, conf2.value)
else:
#traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10)
path = manipulator_motion_plan(base_manip,
manipulator,
conf2.value,
max_iterations=10)
robot.Release(body1)
if path is None:
return
traj = Traj(path)
traj.pose = None
traj.grasp = grasp
yield [(traj, )]
def sample_grasp_traj(pose, grasp):
enable_all(all_bodies, False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans = manip_from_pose_grasp(pose.value, grasp.value)
grasp_conf = solve_inverse_kinematics(manipulator, manip_trans)
if grasp_conf is None:
return
if DISABLE_MOTIONS:
yield [(Conf(grasp_conf), Traj([]))]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(manipulator, pregrasp_trans)
if pregrasp_conf is None:
return
if has_mp():
path = mp_straight_line(robot, grasp_conf, pregrasp_conf)
else:
path = linear_motion_plan(robot, pregrasp_conf)
robot.Release(body1)
if path is None:
return
grasp_traj = Traj(path)
grasp_traj.pose = pose
grasp_traj.grasp = grasp
yield [(Conf(pregrasp_conf), grasp_traj)]
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 _cfree_traj_pose(traj, pose):
enable_all(all_bodies, False)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def sample_free_motion(conf1, conf2):
if DISABLE_MOTIONS:
yield [(make_traj([conf2.value]),)]
return
enable_all(all_bodies, False)
set_manipulator_conf(manipulator, conf1.value)
path = manipulator_motion_plan(
base_manip, manipulator, conf2.value, max_iterations=10)
if path is None:
return
yield [(make_traj(path),)]
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.value:
set_manipulator_conf(manipulator, conf)
if env.CheckCollision(robot, body2):
return False
return True
def _cfree_traj_grasp_pose(traj, grasp, pose):
enable_all(all_bodies, False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, pose.value)
for conf in traj.value:
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1, object_trans_from_manip_trans(
manip_trans, grasp.value))
if env.CheckCollision(body1, body2):
return False
return True
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.value:
set_manipulator_conf(manipulator, conf)
manip_trans = manipulator.GetTransform()
set_pose(body1,
object_trans_from_manip_trans(manip_trans, grasp.value))
if env.CheckCollision(body1, body2):
return False
return True
def sample_holding_motion(conf1, conf2, obj, grasp):
if DISABLE_MOTIONS:
yield [(make_traj([conf2.value], obj=obj, grasp=grasp),)]
return
enable_all(bodies, False)
body = bodies[obj]
body.Enable(True)
set_manipulator_conf(manipulator, conf1.value)
manip_trans = manipulator.GetTransform()
set_pose(body, object_trans_from_manip_trans(manip_trans, grasp.value))
robot.Grab(body)
path = manipulator_motion_plan(
base_manip, manipulator, conf2.value, max_iterations=10)
robot.Release(body)
if path is None:
return
yield [(make_traj(path, obj=obj, grasp=grasp),)]
def sample_grasp_traj(obj, pose, grasp):
enable_all(bodies, False)
body = bodies[obj]
body.Enable(True)
set_pose(body, pose.value)
set_manipulator_conf(manipulator, carry_config)
manip_tform = manip_from_pose_grasp(pose.value, grasp.value)
for base_tform in islice(random_inverse_reachability(ir_model, manip_tform), max_failures):
set_trans(robot, base_tform)
if env.CheckCollision(robot) or robot.CheckSelfCollision():
continue
approach_robot_conf = robot.GetConfigurationValues()
grasp_manip_conf = solve_inverse_kinematics(
manipulator, manip_tform)
if grasp_manip_conf is None:
continue
set_manipulator_conf(manipulator, grasp_manip_conf)
grasp_robot_conf = robot.GetConfigurationValues()
if DISABLE_MOTIONS:
path = [approach_robot_conf,
grasp_robot_conf, approach_robot_conf]
yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))]
return
robot.Grab(body)
pregrasp_tform = manip_tform.dot(
trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(
manipulator, pregrasp_tform)
if pregrasp_conf is None:
continue
path = linear_motion_plan(robot, pregrasp_conf)
robot.Release(body)
if path is None:
continue
yield [(Conf(approach_robot_conf), make_traj(path, obj, pose, grasp))]
return
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 = manip_from_pose_grasp(pose.value, grasp.value)
grasp_conf = solve_inverse_kinematics(
manipulator, manip_trans) # Grasp configuration
if grasp_conf is None:
return
if DISABLE_MOTIONS:
yield [(Conf(grasp_conf), Traj([]))]
return
set_manipulator_conf(manipulator, grasp_conf)
robot.Grab(body1)
pregrasp_trans = manip_trans.dot(trans_from_point(*APPROACH_VECTOR))
pregrasp_conf = solve_inverse_kinematics(
manipulator, pregrasp_trans) # Pre-grasp configuration
if pregrasp_conf is None:
return
# Trajectory from grasp configuration to pregrasp
if has_mp():
path = mp_straight_line(robot, grasp_conf, pregrasp_conf)
else:
path = linear_motion_plan(robot, pregrasp_conf)
#grasp_traj = vector_traj_helper(env, robot, approach_vector)
#grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if path is None:
return
grasp_traj = Traj(path)
grasp_traj.pose = pose
grasp_traj.grasp = grasp
yield [(Conf(pregrasp_conf), grasp_traj)]
def sample_grasp_traj(obj, pose, grasp): # Sample pregrasp config and motion plan that performs a grasp
body = bodies[obj]
def reset_env():
enable_all(bodies, False)
body.Enable(True)
set_pose(body, pose.value)
set_active_manipulator(robot, arm.GetName())
reset_env()
for _ in xrange(max_failures):
gripper_from_obj, gripper_from_pregrasp = grasp.value.sample()
world_from_gripper = manip_from_pose_grasp(pose.value, gripper_from_obj)
world_from_base = next(random_inverse_reachability(ir_model, world_from_gripper))
set_trans(robot, world_from_base)
set_manipulator_conf(arm, carry_arm_conf)
if check_collision(robot):
continue
q = Conf(robot.GetConfigurationValues())
grasp_arm_conf = solve_inverse_kinematics(arm, world_from_gripper)
if grasp_arm_conf is None:
continue
set_manipulator_conf(arm, grasp_arm_conf)
#robot.Grab(body)
pregrasp_arm_conf = solve_inverse_kinematics(arm, world_from_gripper.dot(gripper_from_pregrasp))
if pregrasp_arm_conf is None:
#robot.Release(body)
continue
set_manipulator_conf(arm, pregrasp_arm_conf)
if DISABLE_MOTIONS:
path = [grasp_arm_conf, pregrasp_arm_conf, carry_arm_conf]
t = Prehensile(full_from_active(robot, path), obj, pose, gripper_from_obj)
yield [(q, t)]
reset_env()
return
grasp_path = plan_straight_path(robot, grasp_arm_conf, pregrasp_arm_conf)
#robot.Release(body)
if grasp_path is None:
continue
pregrasp_path = plan_path(base_manip, pregrasp_arm_conf, carry_arm_conf)
if pregrasp_path is None:
continue
t = Prehensile(full_from_active(robot, grasp_path + pregrasp_path[1:]), obj, pose, grasp)
yield [(q, t)]
reset_env()
return
else:
pass
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 sample_grasp_traj(obj, pose, pose2):
enable_all(bodies, False)
body = bodies[obj]
#body.Enable(True) # TODO: fix this
set_pose(body, pose.value)
# TODO: check if on the same surface
point = point_from_pose(pose.value)
point2 = point_from_pose(pose2.value)
distance = length(point2 - point)
if (distance <= 0.0) or (0.6 <= distance):
return
direction = normalize(point2 - point)
orientation = quat_from_angle_vector(angle_from_vector(direction),
[0, 0, 1])
#rotate_direction = trans_from_quat_point(orientation, unit_point())
steps = int(math.ceil(distance / PUSH_MAX_DISTANCE) + 1)
distances = np.linspace(0., distance, steps)
points = [point + d * direction for d in distances]
poses = [pose_from_quat_point(orientation, point) for point in points]
pose_objects = [pose] + map(Pose, poses[1:-1]) + [pose2]
print distance, steps, distances, len(poses)
radius, height = get_mesh_radius(body), get_mesh_height(body)
contact = cylinder_contact(radius, height)
pushes = []
# TODO: I could do all trajectories after all pushes planned
for i in xrange(len(poses) - 1):
p1, p2 = poses[i:i + 2]
# TODO: choose midpoint for base
ir_world_from_gripper = manip_from_pose_grasp(p1, contact)
set_manipulator_conf(arm, carry_arm_conf)
for world_from_base in islice(
random_inverse_reachability(ir_model,
ir_world_from_gripper),
max_failures):
set_trans(robot, world_from_base)
set_manipulator_conf(arm, carry_arm_conf)
if check_collision(robot):
continue
q = Conf(robot.GetConfigurationValues())
push_arm_confs = []
approach_paths = []
for p in (p1, p2):
# TODO: make sure I have the +x push conf
world_from_gripper = manip_from_pose_grasp(p, contact)
set_manipulator_conf(arm, carry_arm_conf)
grasp_arm_conf = solve_inverse_kinematics(
arm, world_from_gripper, collisions=False)
if grasp_arm_conf is None:
break
push_arm_confs.append(grasp_arm_conf)
set_manipulator_conf(arm, grasp_arm_conf)
pregrasp_arm_conf = solve_inverse_kinematics(
arm,
world_from_gripper.dot(gripper_from_pregrasp),
collisions=False)
if pregrasp_arm_conf is None:
break
#if DISABLE_MOTIONS:
if True:
approach_paths.append([
carry_arm_conf, pregrasp_arm_conf, grasp_arm_conf
])
continue
"""
set_manipulator_conf(arm, pregrasp_arm_conf)
grasp_path = plan_straight_path(robot, grasp_arm_conf, pregrasp_arm_conf)
# robot.Release(body)
if grasp_path is None:
continue
pregrasp_path = plan_path(base_manip, pregrasp_arm_conf, carry_arm_conf)
if pregrasp_path is None:
continue
t = Prehensile(full_from_active(robot, grasp_path + pregrasp_path[1:]), obj, pose, grasp)
yield [(q, t)]
reset_env()
return
"""
else:
# Start and end may have different orientations
pq1, pq2 = push_arm_confs
push_path = plan_straight_path(robot, pq1, pq2)
# TODO: make sure the straight path is actually straight
if push_path is None:
continue
po1, po2 = pose_objects[i:i + 2]
ap1, ap2 = approach_paths
m = Push(
full_from_active(robot, ap1),
full_from_active(robot, push_arm_confs),
#full_from_active(robot, push_path),
full_from_active(robot, ap2[::-1]),
obj,
po1,
po2,
contact)
pushes.append(PushMotion(obj, po1, po2, q, m))
break
else:
print 'Failure', len(pushes)
return
print 'Success', len(pushes)
yield pushes
def _execute_traj(confs):
for j, conf in enumerate(confs):
set_manipulator_conf(manipulator, conf)
sleep(0.05)
