def fn(self, edge, object_name, object_pose, holding):
oracle = self.oracle
preprocess_edge(oracle, edge)
all_trans = [
object_trans_from_manip_trans(q.manip_trans,
holding.grasp.grasp_trans)
for q in edge.configs()
] # TODO - cache these transforms
distance = oracle.get_radius2(object_name) + oracle.get_radius2(
holding.object_name)
nearby_trans = [
trans for trans in all_trans if length2(
point_from_trans(trans) -
point_from_pose(object_pose.value)) <= distance
]
if len(nearby_trans) == 0: return False
with oracle.body_saver(object_name):
oracle.set_pose(object_name, object_pose)
with oracle.body_saver(holding.object_name):
for trans in nearby_trans:
set_trans(oracle.get_body(holding.object_name), trans)
if collision(oracle, object_name, holding.object_name):
return True
return False
def linear_push_traj(oracle,
object_name,
initial_pose,
goal_point,
max_failures=LINEAR_PUSH_MAX_FAILURES):
initial_quat = quat_from_pose(initial_pose.value)
initial_point = point_from_pose(initial_pose.value)
total_distance = length(goal_point.value - initial_point)
steps = int(ceil(total_distance / PUSH_MAX_DISTANCE) + 1)
distances = np.linspace(0., total_distance, steps)
direction = normalize(goal_point.value - initial_point)
poses = [initial_pose] + [
Pose(pose_from_quat_point(initial_quat, initial_point + d * direction))
for d in distances[1:]
]
contacts = get_contacts(
oracle, object_name,
quat_transform_point(quat_inv(initial_quat), direction))
pushes = []
for start_pose, end_pose in pairs(poses):
failures = count()
cycled_contacts = cycle(randomize(contacts))
while next(failures) < max_failures:
contact = next(cycled_contacts)
push = Push(oracle.get_geom_hash(object_name), start_pose,
end_pose, contact)
if push.sample(oracle, object_name):
pushes.append(push)
break
else:
return None
return pushes
def get_push_trajs(oracle,
body_name,
initial_pose,
goal_point,
max_failures=LINEAR_PUSH_MAX_FAILURES):
geom_hash = oracle.get_geom_hash(body_name)
goal_pose = first(
lambda pose: np.allclose(point_from_pose(pose.value), goal_point.value
) and oracle.pushes[geom_hash]
(initial_pose, pose) is not None, oracle.pushes[geom_hash])
#print goal_pose, initial_pose not in oracle.pushes[geom_hash], initial_pose
#print oracle.pushes[geom_hash].vertices.values()
#print filter(lambda pose: np.allclose(point_from_pose(pose.value), goal_point.value), oracle.pushes[geom_hash])
if goal_pose is None or initial_pose not in oracle.pushes[
geom_hash]: # NOTE - do not need later check if ensuring the traj is good
with oracle.state_saver():
oracle.set_all_object_poses({body_name: initial_pose})
push_traj = linear_push_traj(oracle,
body_name,
initial_pose,
goal_point,
max_failures=max_failures)
if push_traj is None: return []
for push in push_traj:
oracle.pushes[geom_hash].connect(push.start_pose,
push.end_pose, push)
goal_pose = push_traj[-1].end_pose
return [oracle.pushes[geom_hash](initial_pose, goal_pose)[1]]
def sample_push(self, oracle, object_name):
self.approach_config = sample_approach_config(oracle, self.base_trans)
if self.approach_config is None: return False
oracle.set_pose(object_name, self.start_pose)
self.start_contact_config = sample_contact_config(
oracle, manip_trans_from_pose_contact(self.start_pose,
self.contact))
if self.start_contact_config is None: return False
grab(oracle, object_name)
self.push_traj = workspace_traj(
oracle,
point_from_pose(self.end_pose.value) -
point_from_pose(self.start_pose.value))
release(oracle, object_name)
if self.push_traj is None: return False
self.end_contact_config = Config(
self.start_contact_config.value.copy())
self.end_contact_config.value[get_arm_indices(
oracle)] = self.push_traj.end()
return True
def query_to_edge_push_trajs(oracle, body_name, region_name, start_pose,
grasps):
geom_hash = oracle.get_geom_hash(body_name)
start_point = point_from_pose(start_pose.value)
for pose in filter(lambda p: oracle.on_edge(region_name, body_name, p) and \
oracle.pushes[geom_hash](start_pose, p) is not None,
oracle.pushes[oracle.get_geom_hash(body_name)]):
pap, _ = next(
query_paps(oracle,
body_name,
poses=[pose],
grasps=grasps,
max_failures=15), (None, None))
if pap is None: continue
push_trajs = get_push_trajs(oracle, body_name, start_pose,
Point(point_from_pose(pose.value)))
if len(push_trajs) == 0: continue
for push_traj in push_trajs:
yield pap, push_traj
for pose in random_edge_placements(oracle,
body_name,
region_name,
z=start_point[2],
use_quat=quat_from_pose(
start_pose.value),
bias_point=start_point):
pap, _ = next(
query_paps(oracle,
body_name,
poses=[pose],
grasps=grasps,
max_failures=15), (None, None))
if pap is None: continue
push_trajs = get_push_trajs(oracle, body_name, start_pose,
Point(point_from_pose(pose.value)))
if len(push_trajs) == 0: continue
for push_traj in push_trajs:
yield pap, push_traj