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()
