def __init__(self, problem, env, preload_databases=PRELOAD_DATABASES, debug=DEBUG):
self.problem = problem
self.env = env
if debug: print SEPARATOR
self.setup_env()
self.setup_robot(debug)
self.setup_bodies()
self.setup_regions()
self.setup_bounding_volumes()
self.setup_caches()
self.setup_primatives()
if preload_databases:
for body_name in self.get_objects():
get_grasps(self, body_name)
self.ir_database = get_custom_ir(self.robot)
if debug: print SEPARATOR, '\n', self
###############
# NOTE - can use links similarly to KinBodies. Changing the link transform doesn't affect full body, but it remains relatively attached
self.base = self.robot.GetLink('base_link')
self.torso = self.robot.GetLink('torso_lift_link')
self.gripper = self.robot.GetActiveManipulator().GetEndEffector() # NOTE - the gripper is the red thing inside the hand
#self.necessary_collision = lambda b1, p1, b2, p2: self.aabb_collision(b1, trans_from_pose(p1.value), b2, trans_from_pose(p2.value))
self.necessary_collision = lambda b1, p1, b2, p2: self.sphere_collision(b1, point_from_pose(p1.value), b2, point_from_pose(p2.value))
def pap_ir_samples(
env,
max_failures=100,
max_attempts=INF
): # NOTE - max_failures should be large to prevent just easy placements
from manipulation.inverse_reachability.inverse_reachability import openrave_base_iterator, create_custom_ir
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
num_samples = Counter()
for i, pose in enumerate(
take(random_region_placements(oracle, body_name, [table_name]),
max_attempts)):
if i % 10 == 0:
print 'IR sampling iteration:', i, '| samples:', num_samples
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
if pap.sample(oracle,
body_name,
base_iterator_fn=openrave_base_iterator,
max_failures=max_failures,
check_base=False):
yield pap.manip_trans, pap.base_trans
next(num_samples)
def __init__(self, problem, env, preload_databases=PRELOAD_DATABASES, debug=DEBUG, active_arms=None, reset_robot=True):
self.problem = problem
self.env = env
#if problem.rearrangement:
# self.max_grasps = 1
#else:
# self.max_grasps = 4
if debug: print SEPARATOR
self.setup_env()
self.setup_robot(debug, active_arms, reset_robot)
self.setup_bodies()
self.setup_regions()
self.setup_bounding_volumes()
self.setup_caches()
self.setup_primitives()
if preload_databases:
load_ir_database(self)
if problem.grasps is None:
for body_name in self.get_objects():
get_grasps(self, body_name)
else:
self.grasp_database = {}
for obj_name in problem.grasps:
grasp_key = (self.get_geom_hash(obj_name), USE_GRASP_APPROACH, USE_GRASP_TYPE)
self.grasp_database[grasp_key] = problem.grasps[obj_name]
if debug: print SEPARATOR, '\n', self
###############
# NOTE - can use links similarly to KinBodies. Changing the link transform doesn't affect full body, but it remains relatively attached
self.base = self.robot.GetLink('base_link')
self.torso = self.robot.GetLink('torso_lift_link')
#self.necessary_collision = lambda b1, p1, b2, p2: self.aabb_collision(b1, trans_from_pose(p1.value), b2, trans_from_pose(p2.value))
self.necessary_collision = lambda b1, p1, b2, p2: self.sphere_collision(b1, point_from_pose(p1.value), b2, point_from_pose(p2.value))
def pap_ir_samples(env, max_failures=100, max_attempts=INF): # NOTE - max_failures should be large to prevent just easy placements
from manipulation.inverse_reachability.inverse_reachability import openrave_base_iterator, create_custom_ir
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
for pose in take(random_region_placements(oracle, body_name, [table_name]), max_attempts):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
if pap.sample(oracle, body_name, base_iterator_fn=openrave_base_iterator,
max_failures=max_failures, check_base=False):
yield pap.manip_trans, pap.base_trans
def generator(self, start_vertex, goal_connector, rg):
oracle = self.oracle
if USE_CONFIGURATION == CONFIGURATIONS.DYNAMIC:
IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = True, 2, 1, INF, 1
elif USE_CONFIGURATION == CONFIGURATIONS.FIXED_BRANCHING:
IMMEDIATE, INITIAL, PER_CALL, MAX_CALLS, RECURRENCE = False, 2, 1, 1, 1
NUM_CANDIDATE_POSES = 5
start_config = start_vertex.substate['robot']
if isinstance(goal_connector.condition, HoldingCondition):
object_name = goal_connector.condition.object_name
grasps = get_grasps(oracle, object_name)
else:
holding = goal_connector.condition.substate['holding']
object_name, grasps = holding.object_name, [holding.grasp]
preferred_poses = [start_vertex.substate[object_name]] if start_vertex.substate[object_name] is not False else []
pap_iterator = list(flatten(existing_paps(oracle, object_name, grasps=grasps, poses=preferred_poses))) # NOTE - avoids repeated tries
if len(pap_iterator) == 0:
pap_iterator = chain(query_paps(oracle, object_name, grasps=grasps, poses=preferred_poses,
object_poses=state_obstacles(object_name, start_vertex.substate, oracle), max_failures=15), # start pose & objects
query_paps(oracle, object_name, grasps=grasps, poses=preferred_poses, max_failures=25), # start pose
query_paps(oracle, object_name, grasps=grasps, num_poses=NUM_CANDIDATE_POSES,
object_poses=state_obstacles(object_name, start_vertex.substate, oracle), max_failures=10)) # any pose & objects
#query_paps(oracle, object_name, grasps=[grasp], num_poses=NUM_POSES)) # any pose
else:
pap_iterator = iter(pap_iterator)
def add_edges(n):
generated = Counter()
while int(generated) < n and not (REACHABLE_TERMINATE and goal_connector.reachable):
pap = next(pap_iterator, (None, None))[0]
if pap is None: return False
base_trajs = oracle.plan_base_roadmap(start_config, pap.approach_config)
if base_trajs is None: continue
condition_vertex = rg.vertices.get(Substate({object_name: pap.pose}), None) # Avoids circular poses
if not (condition_vertex is None or condition_vertex.reachable or not condition_vertex.active): continue
#if condition_vertex is None or condition_vertex.reachable or not condition_vertex.active: next(generated)
next(generated)
goal_vertex = rg.vertex(Substate({'holding': Holding(object_name, pap.grasp)}))
goal_vertex.connect(goal_connector)
rg.edge(MovePick(object_name, pap, start_config, base_trajs, oracle)).connect(goal_vertex)
return True
if not IMMEDIATE: yield
for calls in irange(MAX_CALLS):
if PRINT: print '%s-%s: %d'%(self.__class__.__name__, goal_connector.condition, calls)
if not add_edges(PER_CALL if calls!=0 else INITIAL): break
for _ in irange(RECURRENCE): yield
def pap_ir_statistics(env, trials=100):
from manipulation.inverse_reachability.inverse_reachability import display_custom_ir
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
successes = []
for pose in take(random_region_placements(oracle, body_name, [table_name]), trials):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
oracle.set_pose(body_name, pap.pose)
#if pap.sample(oracle, body_name, max_failures=50, base_iterator_fn=openrave_base_iterator, check_base=False):
if pap.sample(oracle, body_name, max_failures=50, check_base=False):
oracle.set_robot_config(pap.grasp_config)
successes.append(int(pap.iterations))
handles = display_custom_ir(oracle, pap.manip_trans)
raw_input('Continue?')
return float(len(successes))/trials, np.mean(successes), np.std(successes)
class StreamFn(FunctionStream
): # TODO - maybe find a better way of doing this?
def function(self, (b, )):
return [(Grasp(b.name, grasp), )
for grasp in get_grasps(self.cond_stream.oracle, b.name)]
def sample_grasp(o):
grasps = get_grasps(oracle, o)
for grasp in grasps:
grasp.obj = o
yield grasps
def sample_grasps(o):
yield get_grasps(oracle, o)
def pap_ir_statistics(env, trials=100):
from manipulation.inverse_reachability.inverse_reachability import display_custom_ir, load_ir_database, \
ir_base_trans, forward_transform, manip_base_values, is_possible_fr_trans, is_possible_ir_trans
from manipulation.pick_and_place import PickAndPlace
oracle = pap_ir_oracle(env)
body_name = oracle.objects[0]
table_name = oracle.tables[0]
convert_point = lambda p: np.concatenate([p[:2], [1.]])
"""
load_ir_database(oracle)
print oracle.ir_aabb
print aabb_min(oracle.ir_aabb), aabb_max(oracle.ir_aabb)
handles = []
vertices = xy_points_from_aabb(oracle.ir_aabb)
print vertices
print np.min(oracle.ir_database, axis=0), np.max(oracle.ir_database, axis=0)
for v in vertices:
handles.append(draw_point(oracle.env, convert_point(v)))
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(draw_line(oracle.env, convert_point(v1), convert_point(v2)))
raw_input('Start?')
"""
successes = []
for pose in take(random_region_placements(oracle, body_name, [table_name]),
trials):
grasp = choice(get_grasps(oracle, body_name))
pap = PickAndPlace(None, pose, grasp)
oracle.set_pose(body_name, pap.pose)
#if pap.sample(oracle, body_name, max_failures=50, base_iterator_fn=openrave_base_iterator, check_base=False):
if pap.sample(oracle, body_name, max_failures=50, check_base=False):
oracle.set_robot_config(pap.grasp_config)
successes.append(int(pap.iterations))
handles = display_custom_ir(oracle, pap.manip_trans)
default_trans = get_trans(oracle.robot)
#vertices = xy_points_from_aabb(aabb_apply_trans(oracle.ir_aabb, pap.manip_trans))
vertices = [
point_from_trans(
ir_base_trans(oracle.robot, pap.manip_trans, v, default_trans))
for v in xy_points_from_aabb(oracle.ir_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env, convert_point(v1), convert_point(v2)))
point_from_inverse = lambda trans: point_from_trans(
np.dot(pap.base_trans, forward_transform(trans)))
for trans in oracle.ir_database:
handles.append(
draw_point(oracle.env,
convert_point(point_from_inverse(trans)),
color=(0, 1, 0, .5)))
#vertices = [point_from_inverse(v) for v in xy_points_from_aabb(oracle.ir_aabb)] # Doesn't have the angle in it...
vertices = [
point_from_trans(np.dot(pap.base_trans, trans_from_base_values(v)))
for v in xy_points_from_aabb(oracle.fr_aabb)
]
for v1, v2 in zip(vertices, vertices[1:] + vertices[-1:]):
handles.append(
draw_line(oracle.env,
convert_point(v1),
convert_point(v2),
color=(0, 1, 0, .5)))
assert is_possible_ir_trans(oracle, pap.manip_trans, pap.base_trans) and \
is_possible_fr_trans(oracle, pap.base_trans, pap.manip_trans)
raw_input('Continue?')
return float(
len(successes)) / trials, np.mean(successes), np.std(successes)
def sample_grasp(b):
grasps = get_grasps(oracle, b)
for grasp in grasps:
grasp.obj = b
yield grasps
def dantam_problem(oracle):
problem = oracle.problem
obj_name = oracle.get_objects()[0]
grasp = get_grasps(oracle, obj_name)[0]
assert problem.initial_poses is not None and problem.known_poses is not None
oracle.initial_poses = problem.initial_poses # TODO - hack to avoid duplicating poses
##########
DO_MOTION = False
max_failures = 50 # 10 | 20 | 40
max_calls = 1 # 1 | INF
iterator = custom_base_iterator # current_base_iterator | custom_base_iterator
def sample_ik(p):
saver = oracle.state_saver()
oracle.set_all_object_poses(
{obj_name: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(obj_name, p, grasp):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(obj_name), p, grasp)
if not pap.sample(oracle,
obj_name,
base_iterator_fn=iterator,
max_failures=max_failures,
sample_vector=DO_MOTION,
sample_arm=DO_MOTION,
check_base=False):
break
yield pap.approach_config
#yield (pap.approach_config, pap)
saver.Restore()
def motion_plan(q1, q2):
yield None
##########
CONF, TRAJ = Ty('CONF'), Ty('TRAJ')
# TODO - assert that all objects have the same geometry
poses = oracle.problem.known_poses + oracle.initial_poses.values(
) # TODO - add goal poses
grasps = [None]
POSE = Ty('POSE', domain=poses + grasps)
OBJ = Ty('OBJ', domain=oracle.get_objects())
R, O, T = 'R', 'O', 'T'
Motion = ConType([CONF, TRAJ, CONF], name='Motion')
Placed = ConType([POSE], name='Placed', test=lambda p: isinstance(p, Pose))
Kin = ConType([POSE, CONF], name='Kin')
PoseCFree = ConType(
[POSE, POSE],
name='PCFree',
test=lambda p1, p2: p1 is None or p2 is None or p1 != p2)
# TODO - could specify axiom creation here
state_vars = [Var(R, CONF), Var(O, POSE, args=[OBJ])]
control_vars = [Var(T, TRAJ)]
##########
o = Par('o', OBJ)
actions = [
Action([Motion(X[R], U[T], nX[R])], name='move'),
Action([Kin(X[O,o], X[R]),
Eq(nX[O,o], None)] + \
[Placed(X[O,ob]) for ob in oracle.get_objects()], name='pick'),
Action([Eq(X[O,o], None), Kin(nX[O,o], X[R]),
Placed(nX[O,o])] + \
[PoseCFree(nX[O,o], X[O,ob]) for ob in oracle.get_objects()],
name='place')]
##########
q1, t, q2 = Par(CONF), Par(TRAJ), Par(CONF)
p = Par(POSE)
samplers = [
#Sampler([Motion(q1, t, q2)], inputs=[q1, q2], gen=lambda a,b: iter([None])),
Sampler([Motion(q1, t, q2)], gen=motion_plan, inputs=[q1, q2]),
#Sampler([Stable(o, p)]),
Sampler([Kin(p, q1)], gen=sample_ik, inputs=[p], domain=[Placed(p)]),
]
##########
initial_state = [Eq(X[R], oracle.initial_config)] + \
[Eq(X[O,obj], pose) for obj, pose in oracle.initial_poses.iteritems()]
obj0 = oracle.get_objects()[0]
goal_constraints = [
#Eq(x[R], oracle.initial_config),
#Eq(x[R], None),
#Contained(reg0, X[O,obj0])
#Eq(X[H,obj0], True),
]
for obj in problem.goal_poses:
assert problem.goal_poses[obj] != 'initial' and problem.goal_poses[
obj] not in oracle.initial_poses
goal_constraints.append(Eq(X[O, obj], problem.goal_poses[obj]))
assert len(problem.goal_regions) == 0
return ConstraintProblem(state_vars, control_vars, actions, samplers,
initial_state, goal_constraints)
def sample_grasp(b):
return iter(get_grasps(oracle, b))
def constraint_problem(oracle):
def region_contains(o, p, r): # TODO - this is kind of strange
return isinstance(p, Pose) and oracle.region_contains(r, o, p)
##########
# TODO - support running None or something for these generators to return fewer than one value
def sample_grasp(b):
return iter(get_grasps(oracle, b))
DO_MOTION = False
max_failures = 50 # 10 | 20 | 40
max_calls = 1 # 1 | INF
def sample_ik(b, g, p):
saver = oracle.state_saver()
oracle.set_all_object_poses(
{b: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(b, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(b), p, g)
if not pap.sample(oracle,
b,
max_failures=max_failures,
sample_vector=DO_MOTION,
sample_arm=DO_MOTION,
check_base=False):
break
yield pap.approach_config
#yield (pap.approach_config, pap)
saver.Restore()
def motion_plan(q1, q2):
yield None
def sample_region(o, r, max_samples=2):
poses = random_region_placements(oracle, o, [r], region_weights=True)
for pose in islice(poses, max_samples):
yield (pose, )
##########
#CONF, POSE, BOOL, TRAJ = DType(), DType(), DType(), DType()
CONF, TRAJ = Ty('CONF'), Ty('TRAJ')
BOOL = Ty('BOOL', domain=[True, False])
FIXED_POSES = True
if FIXED_POSES:
# TODO - assert that all the same geometry
poses = oracle.problem.known_poses + oracle.initial_poses.values(
) # TODO - add goal poses
# TODO - this is also kind of bad
grasps = get_grasps(oracle, oracle.get_objects()[0])
POSE = Ty('POSE', domain=poses + grasps)
else:
POSE = Ty('POSE')
R, O, H, HE, T = 'R', 'O', 'H', 'HE', 'T'
#r, h, t = 'r', 'h', 't' # TODO - make different than parameter
OBJ = Ty('OBJ', domain=oracle.get_objects())
REG = Ty('REG', domain=oracle.goal_regions)
Motion = ConType(
[CONF, TRAJ, CONF], name='Motion'
) # NOTE - types are really just for safety more than anything...
Stable = ConType([OBJ, POSE],
name='Stable',
test=lambda o, p: isinstance(p, Pose))
Grasp = ConType([OBJ, POSE],
name='Grasp',
test=lambda o, p: isinstance(p, GraspTform)
) # TODO - shouldn't need this
# NOTE - I could just evaluate these on initial values or something
# NOTE - this would be like saying all new values must come from a generator (not some external source)
Kin = ConType([OBJ, POSE, CONF, POSE], name='Kin')
#Contained = ConType([REG, POSE], name='Contained', test=region_contains)
#Contained = ConType([OBJ, POSE, REG], name='Contained', test=region_contains, domain=[Stable(o, p)])
Contained = ConType([OBJ, POSE, REG],
name='Contained',
test=region_contains)
# TODO - could specify axiom creation here
state_vars = [
Var(R, CONF),
Var(O, POSE, args=[OBJ]),
Var(H, BOOL, args=[OBJ])
] #+ [Var(HE, BOOL)]
control_vars = [Var(T, TRAJ)]
##########
o = Par('o', OBJ)
# actions = [
# Action([Motion(x[R], u[T], nx[R])], name='move'),
# Action([Stable(o, x[O,o]), Grasp(o, nx[O,o]), Kin(o, nx[O,o], x[R], x[O,o]),
# Eq(x[H], None), Eq(nx[H], o)], name='pick'),
# Action([Grasp(o, x[O,o]), Stable(o, nx[O,o]), Kin(o, x[O,o], x[R], nx[O,o]),
# Eq(x[H], o), Eq(nx[H], None)], name='place')]
#actions = [
# Action([Motion(x[R], u[T], nx[R])], name='move'),
# Action([Stable(o, x[O,o]), Grasp(o, nx[O,o]), Kin(o, nx[O,o], x[R], x[O,o]),
# Eq(x[HE], True), Eq(nx[HE], False), Eq(x[H,o], False), Eq(nx[H,o], True)], name='pick'),
# Action([Grasp(o, x[O,o]), Stable(o, nx[O,o]), Kin(o, x[O,o], x[R], nx[O,o]),
# Eq(x[HE], False), Eq(nx[HE], True), Eq(x[H,o], True), Eq(nx[H,o], False)], name='place')]
actions = [
Action([Motion(X[R], U[T], nX[R])], name='move'),
Action([Stable(o, X[O,o]), Grasp(o, nX[O,o]), Kin(o, nX[O,o], X[R], X[O,o]), Eq(nX[H,o], True)] + \
[Eq(X[H,ob], False) for ob in oracle.get_objects()], name='pick'),
Action([Grasp(o, X[O,o]), Stable(o, nX[O,o]), Kin(o, X[O,o], X[R], nX[O,o]),
Eq(X[H,o], True), Eq(nX[H,o], False)], name='place')]
##########
q1, t, q2 = Par(CONF), Par(TRAJ), Par(CONF)
r, p, g = Par(REG), Par(POSE), Par(POSE)
samplers = [
#Sampler([Motion(q1, t, q2)], inputs=[q1, q2], gen=lambda a,b: iter([None])),
Sampler([Motion(q1, t, q2)], gen=motion_plan, inputs=[q1, q2]),
#Sampler([Stable(o, p)]),
Sampler([Kin(o, g, q1, p)],
gen=sample_ik,
inputs=[o, g, p],
domain=[Stable(o, p), Grasp(o, g)]),
]
if not FIXED_POSES:
samplers += [
Sampler([Contained(o, p, r), Stable(o, p)],
gen=sample_region,
inputs=[o, r]),
Sampler([Grasp(o, g)], gen=sample_grasp, inputs=[o]),
]
# NOTE - discard any sampler not mentioned in start or goal, etc
##########
#initial_state = [Eq(x[R], oracle.initial_config), Eq(x[H], None)] + \
# [Eq(x[O,obj], pose) for obj, pose in oracle.initial_poses.iteritems()]
#initial_state = [Eq(x[R], oracle.initial_config), Eq(x[HE], False)] + \
initial_state = [Eq(X[R], oracle.initial_config)] + \
[Eq(X[O,obj], pose) for obj, pose in oracle.initial_poses.iteritems()] + \
[Eq(X[H,obj], False) for obj in oracle.get_objects()]
# NOTE - can specify facts in the initial state
#initial_state += [Stable(obj, pose) for obj, pose in oracle.initial_poses.iteritems()]
# TODO - should I instead just have a generic test?
obj0 = oracle.get_objects()[0]
#reg0 = oracle.goal_regions[0]
goal_constraints = [
#Eq(x[R], oracle.initial_config),
#Eq(x[R], None),
#Contained(reg0, X[O,obj0])
#Eq(X[H,obj0], True),
]
problem = oracle.problem
# if problem.goal_holding is not None:
# if problem.goal_holding is False:
# goal_constraints.append(HandEmpty())
# elif isinstance(problem.goal_holding, ObjGrasp):
# goal_constraints.append(HasGrasp(C(problem.goal_holding.object_name, BLOCK), C(problem.goal_holding.grasp, GRASP)))
# elif problem.goal_holding in oracle.get_objects():
# goal_constraints.append(Holding(C(problem.goal_holding, BLOCK)))
# else:
# raise Exception()
for obj in problem.goal_poses:
if problem.goal_poses[obj] == 'initial':
problem.goal_poses[obj] = oracle.initial_poses[obj]
elif problem.goal_poses[
obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial)
problem.goal_poses[obj] = oracle.initial_poses[
problem.goal_poses[obj]]
goal_constraints.append(Eq(X[O, obj], problem.goal_poses[obj]))
for obj, region in problem.goal_regions.iteritems():
goal_constraints.append(Contained(obj, X[O, obj], region))
return ConstraintProblem(state_vars, control_vars, actions, samplers,
initial_state, goal_constraints)