def step_physics(self, task: EmbodiedTask, id_agent_obj, *args: Any, **kwargs: Any): r"""Update ``_metric``, this method is called from ``Env`` on each ``step``. Step with physics enabled. """ # convert forward command to linear velocity agent_vel_control = self._sim.get_object_velocity_control(id_agent_obj) agent_vel_control.controlling_lin_vel = True agent_vel_control.controlling_ang_vel = True agent_vel_control.lin_vel_is_local = True agent_vel_control.ang_vel_is_local = True agent_vel_control.linear_velocity = np.float32([0, 0, 0]) agent_vel_control.angular_velocity = np.float32([0, 0, 0]) task.is_stop_called = True return self._sim.get_observations_at()
def step(self, task: EmbodiedTask, *args: Any, **kwargs: Any): r"""Update ``_metric``, this method is called from ``Env`` on each ``step``. """ task.is_stop_called = True # type: ignore return self._sim.get_observations_at() # type: ignore
def reset(self, task: EmbodiedTask, *args: Any, **kwargs: Any): task.is_stop_called = False # type: ignore
def step( self, *args: Any, task: EmbodiedTask, linear_velocity: float, angular_velocity: float, time_step: Optional[float] = None, allow_sliding: Optional[bool] = None, **kwargs: Any, ): r"""Moves the agent with a provided linear and angular velocity for the provided amount of time Args: linear_velocity: between [-1,1], scaled according to config.LIN_VEL_RANGE angular_velocity: between [-1,1], scaled according to config.ANG_VEL_RANGE time_step: amount of time to move the agent for allow_sliding: whether the agent will slide on collision """ if allow_sliding is None: allow_sliding = self._sim.config.sim_cfg.allow_sliding # type: ignore if time_step is None: time_step = self.time_step # Convert from [-1, 1] to [0, 1] range linear_velocity = (linear_velocity + 1.0) / 2.0 angular_velocity = (angular_velocity + 1.0) / 2.0 # Scale actions linear_velocity = self.min_lin_vel + linear_velocity * ( self.max_lin_vel - self.min_lin_vel) angular_velocity = self.min_ang_vel + angular_velocity * ( self.max_ang_vel - self.min_ang_vel) # Stop is called if both linear/angular speed are below their threshold if (abs(linear_velocity) < self.min_abs_lin_speed and abs(angular_velocity) < self.min_abs_ang_speed): task.is_stop_called = True # type: ignore return self._sim.get_observations_at(position=None, rotation=None) angular_velocity = np.deg2rad(angular_velocity) self.vel_control.linear_velocity = np.array( [0.0, 0.0, -linear_velocity]) self.vel_control.angular_velocity = np.array( [0.0, angular_velocity, 0.0]) agent_state = self._sim.get_agent_state() # Convert from np.quaternion to mn.Quaternion normalized_quaternion = agent_state.rotation agent_mn_quat = mn.Quaternion(normalized_quaternion.imag, normalized_quaternion.real) current_rigid_state = RigidState( agent_mn_quat, agent_state.position, ) # manually integrate the rigid state goal_rigid_state = self.vel_control.integrate_transform( time_step, current_rigid_state) # snap rigid state to navmesh and set state to object/agent if allow_sliding: step_fn = self._sim.pathfinder.try_step # type: ignore else: step_fn = self._sim.pathfinder.try_step_no_sliding # type: ignore final_position = step_fn(agent_state.position, goal_rigid_state.translation) final_rotation = [ *goal_rigid_state.rotation.vector, goal_rigid_state.rotation.scalar, ] # Check if a collision occured dist_moved_before_filter = (goal_rigid_state.translation - agent_state.position).dot() dist_moved_after_filter = (final_position - agent_state.position).dot() # NB: There are some cases where ||filter_end - end_pos|| > 0 when a # collision _didn't_ happen. One such case is going up stairs. Instead, # we check to see if the the amount moved after the application of the # filter is _less_ than the amount moved before the application of the # filter. EPS = 1e-5 collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter agent_observations = self._sim.get_observations_at( position=final_position, rotation=final_rotation, keep_agent_at_new_pose=True, ) # TODO: Make a better way to flag collisions self._sim._prev_sim_obs["collided"] = collided # type: ignore return agent_observations