Issues with Reset after Collision Detected

[minwenmao]

Hi Community,

I'm working on training a drone to fly to a target while avoiding collisions. I'm using Articulation.set_external_force_and_torques() to simulate drone dynamics. The action space consists of 4 dimensions: 3 linear accelerations (x, y, z) and 1 yaw rate. Here is how I control the drone using the action.

def _pre_physics_step(self, actions) -> None:
  """
  transfer the actions(yaw rate and linear accelerations) to the forces and torques
  """
  self.actions = actions.clone().clamp(-1.0, 1.0)  # (num_envs, 4)
  # update the forces and torques
  self.forces[:, :, :2] = (
      self.cfg.limit_linear_acceleration
      * self.robot_mass[:, :, None]  # (num_envs, num_bodies, 1)
      * self.actions[:, None, :2]  # (num_envs, 1, 2)
  )
  self.forces[:, :, 2] = (
      self.cfg.limit_linear_acceleration_z
      * self.robot_mass
      * self.actions[:, 2].unsqueeze(1)  # shape: (num_envs, 1)
      + self.robot_weight  # shape: (num_envs, num_bodies)
  )  # add gravity on z axis
  yaw_acc = ((self.actions[:, 3] - self.robot.data.root_com_ang_vel_b[:, 2]) / self.sim.cfg.dt).clamp(
      -self.cfg.limit_angular_velocity, self.cfg.limit_angular_velocity
  )  # calculate yaw acceleration
  self.torques[:, :, 2] = (
      self.cfg.limit_angular_acceleration
      * self.inertia_zz  # shape: (num_envs, num_bodies)
      * yaw_acc.unsqueeze(1)  # shape: (num_envs, 1), broadcasted across bodies
  )

def _apply_action(self) -> None:
  self.robot.set_external_force_and_torque(forces=self.forces, torques=self.torques)

Since roll and pitch are not directly controlled, the drone should not flip unless external disturbances occur.
Here is how to reset:

def _reset_idx(self, env_ids: Sequence[int] | None):
  if env_ids is None:
      env_ids = self.robot._ALL_INDICES
  super()._reset_idx(env_ids)
  self._update_terrain_curriculum(env_ids)
  self._reset_default_pos_curriculum(env_ids)
  self._reset_robot_state(env_ids)
  self.robot.reset(env_ids)
  self.reset_buf[env_ids] = 1

def _update_terrain_curriculum(self, env_ids):
  """
  Update the terrain origins based on the arrival status and reward.
  """
  move_up = self.arrival[env_ids] & (self.reward[env_ids] > 13.0)
  move_down = (~self.arrival[env_ids]) & (self.reward[env_ids] < -50.0)
  self.terrain.update_env_origins(
      env_ids=env_ids,
      move_up=move_up,
      move_down=move_down,
  )

def _reset_default_pos_curriculum(self, env_ids):
  """
  Reset the target positions and orientations based on the terrain difficulty.
  """
  # set the root state to the terrain origin and randomize the yaw
  default_root_state = self.robot.data.default_root_state[env_ids].clone()
  terrain_levels = self.terrain.terrain_levels[env_ids]
  difficult_mask = terrain_levels > self.cfg.difficult_row  # mask for difficult terrains
  # for difficult environment, the default position is set randomly, for other environment, the default position is always the origin
  default_root_state[difficult_mask, 0] = torch.zeros_like(default_root_state[difficult_mask, 0]).uniform_(0.0, 5.0)
  default_root_state[difficult_mask, 1] = torch.zeros_like(default_root_state[difficult_mask, 1]).uniform_(0.0, 0.2)
  default_root_state[:, :3] += self.terrain.env_origins[env_ids]
  default_root_state[:, 2] = torch.ones_like(default_root_state[:, 2]) * 3.0
  self.robot.write_root_pose_to_sim(default_root_state[:, :7], env_ids)
  self.robot.write_root_velocity_to_sim(default_root_state[:, 7:], env_ids)

def _reset_robot_state(self, env_ids):
  """
  Reset the robot's state variables to their default values.
  """
  self.action_queue[env_ids] = torch.zeros_like(self.action_queue[env_ids])
  self.arrival[env_ids] = torch.zeros_like(self.arrival[env_ids])
  self.collision[env_ids] = torch.zeros_like(self.collision[env_ids])
  self.actions[env_ids] = torch.zeros_like(self.actions[env_ids])
  self.depth_image[env_ids] = torch.zeros_like(self.depth_image[env_ids])
  self.forces[env_ids] = torch.zeros_like(self.forces[env_ids])
  self.torques[env_ids] = torch.zeros_like(self.torques[env_ids])

Everything works well before any collision, including:

• Stable flight
• Proper resets
• Progressive environment difficulty

However, once a collision occurs, the drone behaves abnormally:

• After reset, it immediately flips or tumbles
• It becomes unstable even when applying zero actions (no forces/torques)
• It quickly collides again, triggering another reset, and the loop continues

What I’ve tried so far:

• During reset, I set the drone's action to all-zeros for several simulation steps (i.e., “freezing” it) in hopes it would recover from the collision — but this doesn’t help
• Even when the action is frozen, I observe that the linear and angular velocities continue increasing, which never happens before a collision. In the video, the actions, linear velocity and angular velocity are printed in the bottom. When the actions are all-zeros, the velocity and the angular velocity are increasing, and the drone flips.

All the experiments mentioned above are done in play mode, indicating it should not be a problem of the policy, as it can sometimes reach the target , and after collision, we spawn the drone back to the simple environment and it still crashed. From what I can tell, some internal state of the dynamics or physics engine might not be properly reset after a collision. Perhaps residual forces, velocities, or contacts are lingering?

Has anyone experienced this issue before? Any suggestions for fully resetting the drone’s dynamic state (e.g., clearing velocities, contacts, or articulation states) would be greatly appreciated.

Thanks in advance!

[minwenmao]

Problem Solved
The issue ultimately came from the drone's model. Specifically, there was an invisible revolute joint between the base and the small cylinder above it. This cylinder was continuously rotating, which compromised the simplicity and stability of the drone's dynamic model.

After a collision, the state of this joint was not reset, causing unexpected behaviors — including the drone flipping uncontrollably. Once replacing this revolute joint with a fixed joint, the problem was resolved.