add path planners

Author: reiniscimurs

poetry.lock

@@ -15,7 +15,8 @@ dependencies = [
     "pytest (>=8.3.4,<9.0.0)",
     "mkdocs (>=1.6.1,<2.0.0)",
     "mkdocstrings[python] (>=0.29.1,<0.30.0)",
-    "mkdocs-material (>=9.6.11,<10.0.0)"
+    "mkdocs-material (>=9.6.11,<10.0.0)",
+    "scipy (>=1.15.2,<2.0.0)"
 ]
 
 

pyproject.toml

@@ -20,7 +20,7 @@ class SAC(object):
     Args:
         state_dim (int): Dimension of the observation/state space.
         action_dim (int): Dimension of the action space.
-        device (str): PyTorch device (e.g., 'cpu' or 'cuda').
+        device (torch.device): PyTorch device (e.g., 'cpu' or 'cuda').
         max_action (float): Maximum magnitude of actions.
         discount (float): Discount factor for rewards.
         init_temperature (float): Initial entropy temperature.
@@ -72,7 +72,7 @@ def __init__(
         self.state_dim = state_dim
         self.action_dim = action_dim
         self.action_range = (-max_action, max_action)
-        self.device = torch.device(device)
+        self.device = device
         self.discount = discount
         self.critic_tau = critic_tau
         self.actor_update_frequency = actor_update_frequency

robot_nav/models/SAC/SAC.py

@@ -0,0 +1,244 @@
+"""
+
+A* grid planning
+
+author: Atsushi Sakai(@Atsushi_twi)
+        Nikos Kanargias ([email protected])
+
+adapted by: Reinis Cimurs
+
+See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
+
+"""
+
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+import shapely
+from irsim.lib.handler.geometry_handler import GeometryFactory
+
+
+class AStarPlanner:
+
+    def __init__(self, env, resolution):
+        """
+        Initialize A* planner
+
+        env (EnvBase): environment where the planning will take place
+        resolution: grid resolution [m]
+        """
+
+        self.resolution = resolution
+        self.env = env.env
+        self.obstacle_list = self.env.obstacle_list[:]
+        self.min_x, self.min_y = 0, 0
+        self.max_x, self.max_y = (
+            self.env.env_config.parse["world"]["width"],
+            self.env.env_config.parse["world"]["height"],
+        )
+        self.x_width = round((self.max_x - self.min_x) / self.resolution)
+        self.y_width = round((self.max_y - self.min_y) / self.resolution)
+        self.motion = self.get_motion_model()
+
+    class Node:
+        def __init__(self, x, y, cost, parent_index):
+            self.x = x  # index of grid
+            self.y = y  # index of grid
+            self.cost = cost
+            self.parent_index = parent_index
+
+        def __str__(self):
+            return (
+                str(self.x)
+                + ","
+                + str(self.y)
+                + ","
+                + str(self.cost)
+                + ","
+                + str(self.parent_index)
+            )
+
+    def planning(self, sx, sy, gx, gy, show_animation=True):
+        """
+        A star path search
+
+        input:
+            s_x: start x position [m]
+            s_y: start y position [m]
+            gx: goal x position [m]
+            gy: goal y position [m]
+
+        output:
+            rx: x position list of the final path
+            ry: y position list of the final path
+        """
+
+        start_node = self.Node(
+            self.calc_xy_index(sx, self.min_x),
+            self.calc_xy_index(sy, self.min_y),
+            0.0,
+            -1,
+        )
+        goal_node = self.Node(
+            self.calc_xy_index(gx, self.min_x),
+            self.calc_xy_index(gy, self.min_y),
+            0.0,
+            -1,
+        )
+
+        open_set, closed_set = dict(), dict()
+        open_set[self.calc_grid_index(start_node)] = start_node
+
+        while True:
+            if len(open_set) == 0:
+                print("Open set is empty..")
+                break
+
+            c_id = min(
+                open_set,
+                key=lambda o: open_set[o].cost
+                + self.calc_heuristic(goal_node, open_set[o]),
+            )
+            current = open_set[c_id]
+
+            # show graph
+            if show_animation:  # pragma: no cover
+                plt.plot(
+                    self.calc_grid_position(current.x, self.min_x),
+                    self.calc_grid_position(current.y, self.min_y),
+                    "xc",
+                )
+                # for stopping simulation with the esc key.
+                plt.gcf().canvas.mpl_connect(
+                    "key_release_event",
+                    lambda event: [exit(0) if event.key == "escape" else None],
+                )
+                if len(closed_set.keys()) % 10 == 0:
+                    plt.pause(0.001)
+
+            if current.x == goal_node.x and current.y == goal_node.y:
+                print("Find goal")
+                goal_node.parent_index = current.parent_index
+                goal_node.cost = current.cost
+                break
+
+            # Remove the item from the open set
+            del open_set[c_id]
+
+            # Add it to the closed set
+            closed_set[c_id] = current
+
+            # expand_grid search grid based on motion model
+            for i, _ in enumerate(self.motion):
+                node = self.Node(
+                    current.x + self.motion[i][0],
+                    current.y + self.motion[i][1],
+                    current.cost + self.motion[i][2],
+                    c_id,
+                )
+                n_id = self.calc_grid_index(node)
+
+                # If the node is not safe, do nothing
+                if not self.verify_node(node):
+                    continue
+
+                if n_id in closed_set:
+                    continue
+
+                if n_id not in open_set:
+                    open_set[n_id] = node  # discovered a new node
+                else:
+                    if open_set[n_id].cost > node.cost:
+                        # This path is the best until now. record it
+                        open_set[n_id] = node
+
+        rx, ry = self.calc_final_path(goal_node, closed_set)
+
+        return rx, ry
+
+    def calc_final_path(self, goal_node, closed_set):
+        # generate final course
+        rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
+            self.calc_grid_position(goal_node.y, self.min_y)
+        ]
+        parent_index = goal_node.parent_index
+        while parent_index != -1:
+            n = closed_set[parent_index]
+            rx.append(self.calc_grid_position(n.x, self.min_x))
+            ry.append(self.calc_grid_position(n.y, self.min_y))
+            parent_index = n.parent_index
+
+        return rx, ry
+
+    @staticmethod
+    def calc_heuristic(n1, n2):
+        w = 1.0  # weight of heuristic
+        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
+        return d
+
+    def calc_grid_position(self, index, min_position):
+        """
+        calc grid position
+
+        :param index:
+        :param min_position:
+        :return:
+        """
+        pos = index * self.resolution + min_position
+        return pos
+
+    def calc_xy_index(self, position, min_pos):
+        return round((position - min_pos) / self.resolution)
+
+    def calc_grid_index(self, node):
+        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
+
+    def verify_node(self, node):
+        px = self.calc_grid_position(node.x, self.min_x)
+        py = self.calc_grid_position(node.y, self.min_y)
+
+        if px < self.min_x:
+            return False
+        elif py < self.min_y:
+            return False
+        elif px >= self.max_x:
+            return False
+        elif py >= self.max_y:
+            return False
+
+        # collision check
+        if self.check_node(px, py):
+            return False
+
+        return True
+
+    def check_node(self, x, y):
+        node_position = [x, y]
+        shape = {
+            "name": "rectangle",
+            "length": self.resolution,
+            "width": self.resolution,
+        }
+        gf = GeometryFactory.create_geometry(**shape)
+        geometry = gf.step(np.c_[node_position])
+        covered_node = any(
+            [shapely.intersects(geometry, obj._geometry) for obj in self.obstacle_list]
+        )
+        return covered_node
+
+    @staticmethod
+    def get_motion_model():
+        # dx, dy, cost
+        motion = [
+            [1, 0, 1],
+            [0, 1, 1],
+            [-1, 0, 1],
+            [0, -1, 1],
+            [-1, -1, math.sqrt(2)],
+            [-1, 1, math.sqrt(2)],
+            [1, -1, math.sqrt(2)],
+            [1, 1, math.sqrt(2)],
+        ]
+
+        return motion