def fn(self, config, object_name, object_pose, holding):
oracle = self.oracle
if not hasattr(config, 'preprocessed'):
config.processed = True
with oracle.robot_saver():
oracle.set_robot_config(config)
config.saver = oracle.robot_saver()
config.manip_trans = get_manip_trans(oracle)
config.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]]
trans = object_trans_from_manip_trans(config.manip_trans, holding.grasp.grasp_trans)
distance = oracle.get_radius2(object_name) + oracle.get_radius2(holding.object_name)
check_holding = length2(point_from_trans(trans) - point_from_pose(object_pose.value)) <= distance
object_aabb = oracle.get_aabb(object_name, trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(object_aabb)
check_robot = fast_aabb_overlap(object_aabb_min, object_aabb_max, config.aabbs)
if not check_holding or not check_robot: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
if check_robot:
with oracle.robot_saver():
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
config.saver.Restore()
if robot_collision(oracle, object_name):
return True
if check_holding:
with oracle.body_saver(holding.object_name):
set_trans(oracle.get_body(holding.object_name), trans)
if collision(oracle, object_name, holding.object_name):
return True
return False
def fn(self, edge, object_name, object_pose):
oracle = self.oracle
preprocess_edge(oracle, edge)
object_aabb = oracle.get_aabb(object_name,
trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(
object_aabb)
configs = [
q for q in edge.configs()
if fast_aabb_overlap(object_aabb_min, object_aabb_max, q.aabbs)
]
if len(configs) == 0: return False
#distance = oracle.get_radius2('pr2') + oracle.get_radius2(object_name)
#z = get_point(oracle.robot)[-1]
#configs = [q for q in edge.configs() if length2(np.array([q.value[-3], q.value[-2], z]) - point_from_pose(object_pose.value)) <= distance]
#if len(configs) == 0: return False
# TODO - oracle.robot.SetActiveDOFValues(q.value) is the bottleneck: check each object at the same time
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.robot_saver():
CSpace.robot_arm_and_base(
oracle.robot.GetActiveManipulator()).set_active()
with collision_saver(
oracle.env, openravepy_int.CollisionOptions.ActiveDOFs
): # TODO - does this really do what I think?
for q in configs: # TODO - do this with other other collision functions
q.saver.Restore()
if robot_collision(oracle, object_name):
return True
return False
def fn(self, start_config, trajs, object_name, object_pose):
oracle = self.oracle
preprocess_trajs(oracle, start_config, trajs)
object_aabb = oracle.get_aabb(object_name,
trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(
object_aabb)
savers = [
traj.savers[i] for traj in trajs
for i, aabbs in enumerate(traj.aabbs)
if fast_aabb_overlap(object_aabb_min, object_aabb_max, aabbs)
]
if len(savers) == 0: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.robot_saver():
trajs[0].cspace.set_active()
with collision_saver(
oracle.env,
openravepy_int.CollisionOptions.ActiveDOFs):
for saver in savers:
saver.Restore()
if robot_collision(oracle, object_name):
return True
return False
def fn(self, edge, object_name, object_pose):
oracle = self.oracle
preprocess_edge(oracle, edge)
object_aabb = oracle.get_aabb(object_name, trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(object_aabb)
configs = [q for q in edge.configs() if fast_aabb_overlap(object_aabb_min, object_aabb_max, q.aabbs)]
if len(configs) == 0: return False
#distance = oracle.get_radius2('pr2') + oracle.get_radius2(object_name)
#z = get_point(oracle.robot)[-1]
#configs = [q for q in edge.configs() if length2(np.array([q.value[-3], q.value[-2], z]) - point_from_pose(object_pose.value)) <= distance]
#if len(configs) == 0: return False
# TODO - oracle.robot.SetActiveDOFValues(q.value) is the bottleneck: check each object at the same time
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.robot_saver():
CSpace.robot_arm_and_base(oracle.robot.GetActiveManipulator()).set_active()
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs): # TODO - does this really do what I think?
for q in configs: # TODO - do this with other other collision functions
q.saver.Restore()
if robot_collision(oracle, object_name):
return True
return False
def fn(self, config, object_name, object_pose, holding):
oracle = self.oracle
if not hasattr(config, 'preprocessed'):
config.processed = True
with oracle.robot_saver():
oracle.set_robot_config(config)
config.saver = oracle.robot_saver()
config.manip_trans = get_manip_trans(oracle)
config.aabbs = [
(aabb_min(aabb), aabb_max(aabb)) for aabb in
[oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]
]
trans = object_trans_from_manip_trans(config.manip_trans,
holding.grasp.grasp_trans)
distance = oracle.get_radius2(object_name) + oracle.get_radius2(
holding.object_name)
check_holding = length2(
point_from_trans(trans) -
point_from_pose(object_pose.value)) <= distance
object_aabb = oracle.get_aabb(object_name,
trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(
object_aabb)
check_robot = fast_aabb_overlap(object_aabb_min, object_aabb_max,
config.aabbs)
if not check_holding or not check_robot: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
if check_robot:
with oracle.robot_saver():
with collision_saver(
oracle.env,
openravepy_int.CollisionOptions.ActiveDOFs):
config.saver.Restore()
if robot_collision(oracle, object_name):
return True
if check_holding:
with oracle.body_saver(holding.object_name):
set_trans(oracle.get_body(holding.object_name), trans)
if collision(oracle, object_name, holding.object_name):
return True
return False
def preprocess_edge(oracle, edge):
if not hasattr(edge, 'preprocessed'):
edge.preprocessed = True
with oracle.robot_saver():
CSpace.robot_arm_and_base(oracle.robot.GetActiveManipulator()).set_active()
for q in edge.configs():
if not hasattr(q, 'saver'):
oracle.robot.SetActiveDOFValues(q.value)
q.saver = oracle.robot_saver()
q.manip_trans = get_manip_trans(oracle)
q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]]
def preprocess_edge(oracle, edge):
if not hasattr(edge, 'preprocessed'):
edge.preprocessed = True
with oracle.robot_saver():
CSpace.robot_arm_and_base(
oracle.robot.GetActiveManipulator()).set_active()
for q in edge.configs():
if not hasattr(q, 'saver'):
oracle.robot.SetActiveDOFValues(q.value)
q.saver = oracle.robot_saver()
q.manip_trans = get_manip_trans(oracle)
q.aabbs = [(aabb_min(aabb), aabb_max(aabb)) for aabb in [
oracle.compute_part_aabb(part)
for part in USE_ROBOT_PARTS
]]
def preprocess_trajs(oracle, start_config, trajs): # TODO - should store start_config, trajs tuple
if any(not hasattr(traj, 'preprocessed') for traj in trajs):
with oracle.robot_saver():
set_full_config(oracle.robot, start_config.value) # TODO - get rid of this or something
for traj in trajs:
traj.preprocessed = True
traj.cspace.set_active()
traj.savers = [] # TODO - nasty
traj.manip_trans = []
traj.aabbs = []
for q in traj.path():
traj.cspace.body.SetActiveDOFValues(q)
traj.savers.append(oracle.robot_saver())
traj.manip_trans.append(get_manip_trans(oracle))
traj.aabbs.append([(aabb_min(aabb), aabb_max(aabb)) for aabb in [oracle.compute_part_aabb(part) for part in USE_ROBOT_PARTS]])
def fn(self, start_config, trajs, object_name, object_pose):
oracle = self.oracle
preprocess_trajs(oracle, start_config, trajs)
object_aabb = oracle.get_aabb(object_name, trans_from_pose(object_pose.value))
object_aabb_min, object_aabb_max = aabb_min(object_aabb), aabb_max(object_aabb)
savers = [traj.savers[i] for traj in trajs for i, aabbs in enumerate(traj.aabbs)
if fast_aabb_overlap(object_aabb_min, object_aabb_max, aabbs)]
if len(savers) == 0: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.robot_saver():
trajs[0].cspace.set_active()
with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
for saver in savers:
saver.Restore()
if robot_collision(oracle, object_name):
return True
return False
def preprocess_trajs(oracle, start_config,
trajs): # TODO - should store start_config, trajs tuple
if any(not hasattr(traj, 'preprocessed') for traj in trajs):
with oracle.robot_saver():
set_full_config(
oracle.robot,
start_config.value) # TODO - get rid of this or something
for traj in trajs:
traj.preprocessed = True
traj.cspace.set_active()
traj.savers = [] # TODO - nasty
traj.manip_trans = []
traj.aabbs = []
for q in traj.path():
traj.cspace.body.SetActiveDOFValues(q)
traj.savers.append(oracle.robot_saver())
traj.manip_trans.append(get_manip_trans(oracle))
traj.aabbs.append([(aabb_min(aabb), aabb_max(aabb))
for aabb in [
oracle.compute_part_aabb(part)
for part in USE_ROBOT_PARTS
]])