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 get_conf(robot, manip_name): # TODO: treat torso as base
full_conf = robot.GetConfigurationValues()
if manip_name == 'base':
base_conf = base_values_from_full_config(full_conf)
#with robot: # TODO: fix this
# set_active(robot, use_base=True)
# base_conf = robot.GetActiveDOFValues()
return Conf(base_conf)
manipulator = robot.GetManipulator(manip_name)
return Conf(full_conf[manipulator.GetArmIndices()])
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 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 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 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?')