in habitat/tasks/rearrange/sub_tasks/pick_task.py [0:0]
def _gen_start_pos(self, sim, is_easy_init):
target_positions = self._get_targ_pos(sim)
if self.force_set_idx is not None:
sel_idx = self.force_set_idx
else:
sel_idx = np.random.randint(0, len(target_positions))
targ_pos = target_positions[sel_idx]
orig_start_pos = sim.pathfinder.snap_point(targ_pos)
state = sim.capture_state()
start_pos = orig_start_pos
forward = np.array([1.0, 0, 0])
dist_thresh = 0.1
did_collide = False
# Add noise to the base position and angle for a collision free
# starting position
timeout = 1000
attempt = 0
while attempt < timeout:
attempt += 1
start_pos = orig_start_pos + np.random.normal(
0, self._config.BASE_NOISE, size=(3,)
)
rel_targ = targ_pos - start_pos
angle_to_obj = get_angle(forward[[0, 2]], rel_targ[[0, 2]])
if np.cross(forward[[0, 2]], rel_targ[[0, 2]]) > 0:
angle_to_obj *= -1.0
targ_dist = np.linalg.norm((start_pos - orig_start_pos)[[0, 2]])
is_navigable = is_easy_init or sim.pathfinder.is_navigable(
start_pos
)
if targ_dist > dist_thresh or not is_navigable:
continue
sim.set_state(state)
sim.robot.base_pos = start_pos
# Face the robot towards the object.
rot_noise = np.random.normal(0.0, self._config.BASE_ANGLE_NOISE)
sim.robot.base_rot = angle_to_obj + rot_noise
# Make sure the robot is not colliding with anything in this
# position.
for _ in range(100):
sim.internal_step(-1)
did_collide, details = rearrange_collision(
self._sim,
self._config.COUNT_OBJ_COLLISIONS,
ignore_base=False,
)
if is_easy_init:
# Only care about collisions between the robot and scene.
did_collide = details.robot_scene_colls != 0
if did_collide:
break
if not did_collide:
break
if attempt == timeout - 1 and (not is_easy_init):
start_pos, angle_to_obj, sel_idx = self._gen_start_pos(sim, True)
sim.set_state(state)
return start_pos, angle_to_obj, sel_idx