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 get_values(self, **kwargs):
oracle = self.cond_stream.oracle
b, p, g = self.inputs
if self.calls >= self.max_calls:
self.enumerated = True
with oracle.state_saver():
oracle.set_all_object_poses({b.name: p.value})
if oracle.approach_collision(b.name, p.value, g.value):
return []
pap = PickAndPlace(oracle.get_geom_hash(b.name), p.value,
g.value)
if not pap.sample(oracle,
b.name,
max_failures=self.max_failures,
sample_vector=DO_MOTION,
sample_arm=DO_MOTION,
check_base=False):
return []
#self.oracle.add_pap(self.obj, pap)
q, t = Config(pap.approach_config), Trajectory(b.name, pap)
initial_atoms = []
for obj in oracle.get_objects():
if b.name != obj and obj not in oracle.introduced_objects:
block, pose = C(obj,
BODY), Pose(obj,
oracle.initial_poses[obj])
if are_colliding(pose, t, oracle):
initial_atoms += get_object_initial_atoms(
obj, oracle)
#initial_atoms += [AtPose(block, pose), IsPose(block, pose)]
return [IsKin(b, p, g, q, t)] + initial_atoms
def sample_motion(o, p, g, max_calls=1, max_failures=50):
oracle.set_all_object_poses({o: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(o, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
if not pap.sample(oracle, o, max_failures=max_failures,
sample_vector=DO_ARM_MOTION, sample_arm=DO_ARM_MOTION, check_base=CHECK_BASE):
break
pap.obj = o
yield [(pap.approach_config, pap)]
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_ARM_MOTION, sample_arm=DO_ARM_MOTION, check_base=CHECK_BASE):
break
pap.obj = b
yield [(pap.approach_config, pap)]
saver.Restore()
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 sample_motion(o, p, g, max_calls=1, max_failures=50):
oracle.set_all_object_poses(
{o: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(o, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
if not pap.sample(oracle,
o,
max_failures=max_failures,
sample_vector=False,
sample_arm=False,
check_base=False):
break
#pap.obj = o
yield [(get_base_conf(pap.grasp_config),
get_arm_conf(pap.grasp_config))]
def get_next(self, **kwargs):
oracle = self.cond_stream.oracle
b, p, g = self.inputs
self.enumerated = True # TODO - remove this later
with oracle.state_saver():
oracle.set_all_object_poses({b.name: p.value})
if oracle.approach_collision(b.name, p.value, g.value):
return []
pap = PickAndPlace(oracle.get_geom_hash(b.name), p.value,
g.value)
if not pap.sample(oracle,
b.name,
max_failures=10,
sample_vector=DO_MOTION,
sample_arm=DO_MOTION,
check_base=False):
return []
#self.oracle.add_pap(self.obj, pap)
return [(Trajectory(b.name, pap), )]
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 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)
def get_values(self, **kwargs):
oracle = self.cond_stream.oracle
b, p, m, g = self.inputs
manip_name = m.name + 'arm'
oracle.robot.SetActiveManipulator(
manip_name
) # TODO - reflect base pose for rightarm (although right now it is fairly symmetric
#oracle.robot.GetActiveManipulator().SetIkSolver(oracle.ikmodels[manip_name].iksolver)
#ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=self.robot, iktype=IkParameterization.Type.Transform6D,
# forceikfast=True, freeindices=None, freejoints=None, manip=self.robot.GetManipulator(name))
if self.calls >= self.max_calls:
self.enumerated = True
with oracle.state_saver():
oracle.set_all_object_poses({b.name: p.value})
if oracle.approach_collision(b.name, p.value, g.value):
return []
pap = PickAndPlace(oracle.get_geom_hash(b.name), p.value,
g.value)
if not pap.sample(oracle,
b.name,
max_failures=self.max_failures,
sample_vector=DO_MOTION,
sample_arm=DO_MOTION,
check_base=False):
return []
#self.oracle.add_pap(self.obj, pap)
q, t = Config(pap.approach_config), Trajectory(b.name, pap)
initial_atoms = []
for obj in oracle.get_objects():
if b.name != obj and obj not in oracle.introduced_objects:
block, pose = C(obj,
BODY), Pose(obj,
oracle.initial_poses[obj])
if are_colliding(pose, t, oracle):
initial_atoms += get_object_initial_atoms(
obj, oracle)
#initial_atoms += [AtPose(block, pose), IsPose(block, pose)]
return [IsKin(b, p, m, g, q, t)] + initial_atoms
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)
