occant_baselines/rl/ans.py [552:593]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
            forward_step = self.config.LOCAL_POLICY.AGENT_DYNAMICS.forward_step
            for i in range(self.nplanners):
                prev_action_i = observations["prev_actions"][i, 0].item()
                # If not forward action, skip
                if prev_action_i != 0:
                    continue
                x1, y1 = asnumpy(self.states["prev_map_position"][i]).tolist()
                x2, y2 = asnumpy(self.states["curr_map_position"][i]).tolist()
                t2 = global_pose[i, 2].item() - math.pi / 2
                if abs(x1 - x2) < 1 and abs(y1 - y2) < 1:
                    self.states["col_width"][i] += 2
                    self.states["col_width"][i] = min(self.states["col_width"][i], 9)
                else:
                    self.states["col_width"][i] = 1
                dist_trav_i = math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2) * s
                # Add an obstacle infront of the agent if a collision happens
                if dist_trav_i < 0.7 * forward_step:  # Collision
                    length = 2
                    width = int(self.states["col_width"][i].item())
                    buf = 3
                    cmH, cmW = self.states["collision_map"][i].shape
                    for j in range(length):
                        for k in range(width):
                            wx = (
                                x2
                                + ((j + buf) * math.cos(t2))
                                + ((k - width / 2) * math.sin(t2))
                            )
                            wy = (
                                y2
                                + ((j + buf) * math.sin(t2))
                                - ((k - width / 2) * math.cos(t2))
                            )
                            wx, wy = int(wx), int(wy)
                            if wx < 0 or wx >= cmW or wy < 0 or wy >= cmH:
                                continue
                            self.states["collision_map"][i, wy, wx] = 1
        # Update visitation maps
        for i in range(self.nplanners):
            mx, my = asnumpy(self.states["curr_map_position"][i]).tolist()
            mx, my = int(mx), int(my)
            self.states["visited_map"][i, my - 2 : my + 3, mx - 2 : mx + 3] = 1
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -



occant_baselines/rl/ans.py [1071:1112]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
            forward_step = self.config.LOCAL_POLICY.AGENT_DYNAMICS.forward_step
            for i in range(self.nplanners):
                prev_action_i = observations["prev_actions"][i, 0].item()
                # If not forward action, skip
                if prev_action_i != 0:
                    continue
                x1, y1 = asnumpy(self.states["prev_map_position"][i]).tolist()
                x2, y2 = asnumpy(self.states["curr_map_position"][i]).tolist()
                t2 = global_pose[i, 2].item() - math.pi / 2
                if abs(x1 - x2) < 1 and abs(y1 - y2) < 1:
                    self.states["col_width"][i] += 2
                    self.states["col_width"][i] = min(self.states["col_width"][i], 9)
                else:
                    self.states["col_width"][i] = 1
                dist_trav_i = math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2) * s
                # Add an obstacle infront of the agent if a collision happens
                if dist_trav_i < 0.7 * forward_step:  # Collision
                    length = 2
                    width = int(self.states["col_width"][i].item())
                    buf = 3
                    cmH, cmW = self.states["collision_map"][i].shape
                    for j in range(length):
                        for k in range(width):
                            wx = (
                                x2
                                + ((j + buf) * math.cos(t2))
                                + ((k - width / 2) * math.sin(t2))
                            )
                            wy = (
                                y2
                                + ((j + buf) * math.sin(t2))
                                - ((k - width / 2) * math.cos(t2))
                            )
                            wx, wy = int(wx), int(wy)
                            if wx < 0 or wx >= cmW or wy < 0 or wy >= cmH:
                                continue
                            self.states["collision_map"][i, wy, wx] = 1
        # Update visitation maps
        for i in range(self.nplanners):
            mx, my = asnumpy(self.states["curr_map_position"][i]).tolist()
            mx, my = int(mx), int(my)
            self.states["visited_map"][i, my - 2 : my + 3, mx - 2 : mx + 3] = 1
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -