def update_base(self):
ctrl_freq = self._sim.ctrl_freq
before_trans_state = self._capture_robot_state(self._sim)
trans = self._sim.robot.sim_obj.transformation
rigid_state = habitat_sim.RigidState(
mn.Quaternion.from_matrix(trans.rotation()), trans.translation)
target_rigid_state = self.base_vel_ctrl.integrate_transform(
1 / ctrl_freq, rigid_state)
end_pos = self._sim.step_filter(rigid_state.translation,
target_rigid_state.translation)
target_trans = mn.Matrix4.from_(
target_rigid_state.rotation.to_matrix(), end_pos)
self._sim.robot.sim_obj.transformation = target_trans
if not self._config.get("ALLOW_DYN_SLIDE", True):
# Check if in the new robot state the arm collides with anything. If so
# we have to revert back to the previous transform
self._sim.internal_step(-1)
colls = self._sim.get_collisions()
did_coll, _ = rearrange_collision(colls, self._sim.snapped_obj_id,
False)
if did_coll:
# Don't allow the step, revert back.
self._set_robot_state(self._sim, before_trans_state)
self._sim.robot.sim_obj.transformation = trans
def get_collisions(
self,
count_obj_colls: bool,
ignore_names: Optional[List[str]],
verbose: bool,
):
return rearrange_collision(
self._sim,
count_obj_colls,
ignore_names=ignore_names,
verbose=verbose,
get_extra_coll_data=True,
)
def _gen_start_pos(self, sim, is_easy_init):
target_positions = self._get_targ_pos(sim)
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, ))
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.
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
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
def get_cur_collision_info(self) -> CollDetails:
_, coll_details = rearrange_collision(
self._sim,
self._config.COUNT_OBJ_COLLISIONS,
)
return coll_details