//  [python]
def step(self, step: int, actions: torch.Tensor) -> None:
        """ Step in the environment

        Args:
            step: the step number, start from 1
            actions: action tensor
        """
        actions = actions[:, 0:1, :] # only consider 1 actions

        ## execute the action and compute the next state
        ## 1. normalize the step length of action
        B, K, D = actions.shape # <B, k, D>
        actions_norm = torch.norm(actions, dim=-1)
        mask = actions_norm > self.robot_radius # Important !!!
        actions[mask, ...] *= (self.robot_radius / actions_norm[mask]).unsqueeze(-1)

        ## 2.check the validity of actions
        actions_valid = torch.zeros_like(actions) # <B, 1, D>
        actions_find = self.end.clone() # <B>
        for angle in self.rotate_angles:
            rot_mat = torch.tensor(np.array([[cos(angle), -sin(angle)], 
                [sin(angle), cos(angle)]]), dtype=actions.dtype, device=self.device)
            
            actions_rotate = torch.mm(
                rot_mat, # <2, 2>
                actions.squeeze(1).t() # <2, B>
            ).t().unsqueeze(1) # <B, 1, D>
            
            action_valid_mask = self.verify_actions(actions_rotate)
            action_valid_mask = torch.logical_and(action_valid_mask, ~actions_find)

            ## debug
            # for i in range(B):
            #     S = trimesh.Scene()
            #     scene_pc = self.scene_pos[i].cpu().numpy()
            #     S.add_geometry(trimesh.PointCloud(vertices=scene_pc))
            #     pos = self.state.cpu().numpy()[i:i+1].squeeze()
            #     pos_mesh = create_trimesh_node(pos, radius=0.08)
            #     S.add_geometry(pos_mesh)
            #     S.add_geometry(trimesh.creation.axis())
            #     S.show()
            # print(self.state)

            actions_valid[action_valid_mask, ...] = actions_rotate[action_valid_mask, ...]
            actions_find = torch.logical_or(action_valid_mask, actions_find)

            if torch.all(actions_find):
                break

        ## 3.compute next state
        next_state = actions_valid + self.state
        self.observation_queue.push(actions_valid)

        ## 4. update the state
        self.state[~self.end, ...] = next_state[~self.end, ...]
        for i in range(self.batch):
            self.trajectory[i].append(self.state[i].clone())
        
        ## debug
        # for i in range(B):
        #     S = trimesh.Scene()
        #     scene_pc = self.scene_pos[i].cpu().numpy()
        #     S.add_geometry(trimesh.PointCloud(vertices=scene_pc))
        #     pos = self.state.cpu().numpy()[i:i+1].squeeze()
        #     pos_mesh = create_trimesh_node(pos, radius=0.08)
        #     S.add_geometry(pos_mesh)
        #     S.add_geometry(trimesh.creation.axis())
        #     S.show()
        # print(self.state)
        # print(self.trajectory)
        
        ## 5. check arriving the target position
        dist = torch.norm(self.state.squeeze(1) - self.target, dim=-1)
        arrived = dist < self.arrive_threshold

        self.trajectory_length[torch.logical_and(arrived, ~self.end)] = step
        self.end = torch.logical_or(self.end, arrived)